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The  views  and  conclusions  contained  In  this  document  are  those  of  the  authors 
and  should  not  be  interpreted  as  representing  the  official  policies,  either 
expressed  or  Implied,  of  the  Defense  Advanced  Research  Projects  Agency  or  the 
II. S.  Government. 


1.  INTRODUCTION  AND  SUMMARY 

Research  under  this  contract  began  on  October  1,  1981.  As  planned,  the 
first  two  years  of  work  were  primarily  devoted  to  design  activities  supported 
both  by  computer  simulation  studies  and  by  experimental  Investigations  with 
major  subsystems.  Several  laboratory -scale  vehicles  were  also  constructed  In 
this  time  period  to  permit  validation  of  algorithms  and  evaluation  of  compo¬ 
nents  and  concepts.  This  work  was  all  completed  on-time  and  within  budget. 
Details  can  be  found  In  our  Quarterly  R  &  D  Status  Reports,  and  In  our  pre¬ 
vious  Semi-Annual  Technical  Reports.  A  listing  of  milestones  actually 
achieved  during  our  first  two  years  of  research  Is  attached  to  our  Quarterly 
R  &  0  Status  Report  for  the  period  October  1,  1983  through  December  31,  1983. 

By  the  beginning  of  the  period  covered  by  this  report,  conceptual  design 
of  the  ASV-84  was  essentially  complete.  Per  the  original  project  plan,  dated 
May  13,  1981,  the  first  half  of  FY  84,  the  period  covered  by  this  report,  has 
been  devoted  to  an  Intensive  effort  centered  on  subsystem  detailed  design  and 
manufacturing,  as  well  as  on  Initial  steps  toward  subsystem  integration.  The 
details  of  our  accompl ishments  In  these  areas  are  included  in  this  report  and 
in  previously  submitted  R  4  D  Status  Reports  for  the  period.  While  this  work 
has  revealed  no  significant  defects  in  the  overall  ASV-84  system  design,  some 
delays  In  manufacturing  have  been  encountered  for  a  variety  of  reasons.  This 
matter  Is  treated  In  detail  In  our  letter  to  DARPA  dated  February  27,  1984. 

As  a  consequence  of  these  factors,  and  subsequent  delays  reported  In  later 
correspondence,  we  no  longer  anticipate  that  significant  testing  of  the  ASV-84 
vehicle  can  be  completed  under  the  current  contract.  Rather,  we  are  concen¬ 
trating  our  efforts  on  completing  vehicle  construction  to  the  greatest  extent 
possible  during  the  current  fiscal  year. 


Per  recent  meetings  with  DARPA,  our  goal  is  now  to  complete  final  assem¬ 
bly  of  the  ASV-84  vehicle  In  calendar  year  1984.  A  proposal  detailing  this 
activity  as  well  as  subsequent  phases  of  vehicle  testing,  evaluation,  and 
modification  has  been  favorably  reviewed  by  DARPA.  As  of  the  time  of  this 
writing,  a  contract  covering  this  work  Is  in  the  final  stages  of  negotiation. 

2.  MECHANICAL  SYSTEM  RESEARCH 

2.1  Cockpit  Structure 

The  design  and  manufacture  of  the  cockpit  structure  was  completed  during 
this  reporting  period.  The  structure  is  shown  mated  to  the  body  frame  in 
Figure  1.  The  cockpit  frame  has  subsequently  been  delivered  to  the  University 
of  Wisconsin  to  be  fitted  with  displays  and  controls.  The  frame  dimensions 
used  for  the  cockpit  represent  a  compromise  between  the  space  demanded  by  an 
ergonomically  Ideal  design  and  the  necessity  to  minimize  the  size  of  the 
structure  to  avoid  excessive  overall  weight  and  forward  displacement  of  the 
center  of  mass  of  the  vehicle  relative  to  the  legs.  Specifically,  the  weight 
of  the  structure  increases  with  fore-aft  length  at  a  rate  faster  than  Its 
second  power  because  Increasing  the  length  also  Increases  the  maximum  bending 
moment  which  can  be  applied  to  It  by  a  given  load.  For  operator  protection 
purposes.  It  Is  necessary  to  design  for  a  crash  In  which  the  vehicle  descends 
from  maximum  height  to  a  position  In  which  It  Is  supported  only  on  the  extreme 
front  of  the  cockpit  structure,  and  on  the  rear  legs.  In  the  event  that  the 
present  cockpit  dimensions  prove  to  be  so  small  as  to  cause  operator  incon¬ 
venience  which  would  hinder  the  vehicle  test  program,  a  second  cockpit  will  be 
designed  as  a  retrofit.  This  Is  easily  done  because  the  cockpit  has  been 
designed  as  an  easily  separable  module,  both  structurally  and  electronically. 


Figure  1.  Completed  Cab  Structure  Mated  to  Vehicle  Frame 
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The  engine  cooling  system  has  developed  to  the  point  that  a  continuous 
output  of  70  hp  Is  possible  without  overheating.  Achieving  this  performance 
represented  a  substantial  problem  since,  although  the  engine  Is  rated  at  90  hp 
peak,  as  a  motorcycle  engine  It  Is  only  designed  for  a  fraction  of  that  as 
continuous  output,  and,  moreover.  It  Is  designed  to  operate  In  a  rapidly 
moving  air  stream.  To  provide  for  stationary  operation,  two  cooling  systems 
have  been  designed  and  tested,  and  will  be  Installed  on  the  vehicle  engine. 

The  first  of  these  Is  a  forced  air  system  consisting  of  a  fan  and  a  cowling  to 
distribute  the  air  over  the  engine.  The  second  Is  a  water  Injection  system. 

It  was  Initially  felt  that  sufficient  cooling  for  continuous  high-power  output 
would  not  be  attainable  by  using  a  forced  air  system  alone.  For  this  reason, 
the  water  Injection  system,  based  on  aircraft  engine  technology,  was  designed. 
The  water  Is  Injected  at  a  metered  rate  Into  the  Intake  manifold  through  four 
ports  spaced  to  distribute  It  evenly  among  the  four  cylinders.  Dynamometer 
tests  have  shown,  however,  that  the  ful ly -developed  forced  air  system  Is  capa¬ 
ble  of  maintaining  allowable  head  temperatures  at  continuous  output  levels  in 
excess  of  60  hp.  Therefore,  we  expect  to  be  able  to  operate  using  the  forced 
air  system  alone  In  normal  conditions.  The  water  Injection  system  Is  very 
compact  and  has  also  proved  effective  In  dynamometer  tests.  We  therefore  plan 
to  Install  It  as  a  back-up  cooling  system  which  will  be  automatically  acti¬ 
vated  If  engine  head  temperature  exceeds  a  pre-set  value.  Figure  2  shows  the 
engine  set  up  on  a  hydraulic  dynamometer.  The  cowling  can  be  seen.  Also 
evident  Is  an  oil  cooler  which  will  be  part  of  the  Installation.  The  large 
duct  In  the  photograph  was  used  to  deliver  air  from  a  fan  which  will  not  be 
used  on  the  vehicle.  The  fan  to  be  Installed  will  be  adjacent  to  the  engine. 


2.3  Prototype  Leg  Tests 

The  prototype  leg  was  tested  at  speeds  closely  approximating  those  which 
will  be  used  In  vehicle  operation.  Control  software  was  developed  to  move  the 
foot  along  a  desired  trajectory  In  the  return  phase  of  the  cycle,  and  to  main¬ 
tain  a  desired  contact  pressure  during  the  contact  phase.  A  videotape  of 
these  tests  has  previously  been  supplied  to  DARPA.  The  leg  has  bfeen  cycled  at 
frequencies  up  to  1.4  Hz.  Although  the  leg  stroke  used  was  shorter  than  will 
be  used  at  comparable  speeds  on  the  vehicle,  due  to  the  limitations  of  the 
test  stand,  the  tests  closely  simulated  vehicle  operating  conditions  In  all 
other  respects.  In  particular,  the  equivalent  ground  speed  attained  at  1.2 
Hz.  was  approximately  5  mph,  the  designed  cruise  speed,  and  that  attained  at 

1.4  Hz.  was  close  to  8  mph,  the  designed  maximum  speed.  Figure  3  shows  a 
measured  foot  trajectory  and  curves  of  horizontal  and  vertical  velocity  versus 
time  for  a  typical  leg  test.  The  curves  actually  show  actuator  displacements 
and  rates  with  inches  used  as  the  unit  of  distance.  Thus,  the  lift  actuator 
displacement  and  rate  must  be  multiplied  by  the  pantograph  factor  of  4  to  get 
vertical  foot  displacement  and  rate.  Correspondingly,  the  drive  data  must  be 
multiplied  by  5  to  get  horizontal  foot  displacement  and  rate. 

During  the  course  of  these  tests,  one  of  the  hydraulic  lines  of  the  foot 
attitude  maintenance  system  was  damaged,  leading  to  a  progressive  degradation 
In  functioning  of  that  system  and,  ultimately,  to  complete  cessation  of  func¬ 
tion.  It  was  very  evident  in  the  tests  that  the  foot  attitude  system  is 
essential  to  high  speed  leg  operation.  Cyclic  operation  at  frequencies  above 
0.6  Hz.  without  the  foot  attitude  system  led  to  large,  uncontrolled  swings  of 
the  foot  about  the  ankle,  and  to  failure  of  the  foot  to  assume  a  sole  flat 
position  during  contact.  The  problem  with  the  attitude  system  was  minor  and 
will  be  avoided  In  the  permanent  Installation. 
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Figure  3.  Position  and  Velocity  Response  of  Prototype 
Leg  Measured  at  Actuator 


2.4  Leg  Design  and  Fabrication 

During  this  period  the  detailed  design  of  the  leg  links,  and  of  the 
joints  which  connect  them  was  completed.  Manufacture  has  commenced  with  the 
aluminum  plate  from  which  the  links  will  be  fabricated  having  been  cut  out. 
Welding  of  the  leg  links  is  in  progress.  The  leg  boxes,  which  carry  the 
guideways  for  the  linear  bearings  on  which  the  leg  links  are  mounted,  and  the 
mounts  for  the  actuators  and  the  lift  and  drive  pumps,  remain  to  be  detailed. 

2.5  Flywheel  Tests 

Personnel  from  OSU  provided  technical  support  to  Owens  Corning  during 
flywheel  spin  tests  conducted  during  this  period.  This  was  necessitated  by  a 
management  decision  at  Owens  Corning  which  withdrew  support  from  these  tests. 
Considerable  difficulties  were  encountered  due  to  the  Owens  Corning  spin  pit 
not  being  designed  to  test  flywheels  in  their  housings.  It  is  necessary  to 
test  the  University  of  Wisconsin  flywheel  design  inside  its  housing  in  order 
to  evaluate  its  evacuation  system  and  to  discover  any  vibrational  modes  which 
might  arise  from  the  bearing  mount  configuration.  Problems  also  arose  in  the 
testing  due  to  flooding  of  the  case  with  oil,  and  due  to  the  inadequate  low 
speed  torque  of  the  air  turbine  used  to  power  the  rig.  Vibrations,  which 
limited  the  speed  which  could  be  achieved,  suggested  balancing,  or  structural 
resonance  problems. 

It  should  be  noted  that  this  work  does  not  appear  in  the  work  statement 
of  our  contract.  It  was  undertaken  as  work  essential  to  ensure  timely  com¬ 
pletion  of  our  own  tasks,  since  assembly  and  testing  of  the  vehicle  power 
package  is  dependent  on  successful  completion  of  the  flywheel  test  program. 
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ELECTRICAL  SYSTEM  RESEARCH 


3.1  Precision  Footing  Control 

Control  of  body  motion  with  six  degrees  of  freedom  in  precision  footing 
mode  has  been  implemented  and  tested  on  the  breadboard  ASV-84  vehicle  com¬ 
puter.  For  these  tests,  the  PDP-11/70  computer  was  used  to  simulate  the 
vehicle  kinematics  including  the  cranked  shank  configuration  of  the  prototype 
leg  presented  in  our  previous  Semi-Annual  Report,  dated  May,  1984.  Body  and 
leg  motion  were  represented  in  real  time  during  these  tests  by  means  of  a 
vector  graphics  display  system  using  a  "wi re -frame"  vehicle  mode).  A  more 
easily  understood  shaded  color  graphics  representation  was  subsequently  pro¬ 
duced  by  single-frame  motion  picture  photography.  A  videotape  copy  of  this 
motion  picture  has  been  provided  to  DARPA.  Figure  4  presents  a  single  frame 
from  this  videotape. 

While  studies  of  precision  footing  control  of  body  motion  can  be  accom¬ 
plished  only  by  simulation  prior  to  completion  of  the  entire  ASV-84  vehicle, 
manual  control  of  individual  legs  can  be  Investigated  using  the  physical  hard 
ware  of  the  prototype  leg.  This  has  been  done  with  entirely  satisfactory 
results.  Specifically,  using  a  three-axis  joystick  as  an  input  device,  the 
prototype  leg  has  been  operated  in  Cartesian  coordinates  analogously  to  a  pre 
cision  manipulator.  An  accuracy  of  better  than  1/4  inch  in  foot  placement  is 
easily  achieved  in  this  mode  of  operation.  A  videotape  showing  such  testing 
has  previously  been  furnished  to  DARPA. 

3.2  Omnidirectional  Vehicle  Control 

The  ASV-84  vehicle  mechanical  design  has  been  optimized  to  provide  a 
high-speed  forward-dl rected  "dash"  mode  of  locomotion.  It  therefore  is  of  a 
relatively  slender  configuration  in  comparison  to  the  previous  laboratory- 
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Figure  4.  Single  Frame  from  Videotape  Showing  Precision  Footing  Control  of 
Simulated  ASV-84  Vehicle  by  Breadboard  Computer 


report.  It  is  felt  that  the  algorithms  presented  in  this  dissertation  provide 
one  entirely  satisfactory  approach  to  realization  of  close  maneuvering  for 
experimental  evaluation  of  the  ASV-84  vehicle.  Alternative  algorithms  are 
currently  under  investigation. 

The  principal  features  of  the  algorithms  presented  in  the  appendix  are 
as  follows: 

1.  The  operator  may  select  gaits  in  which  three,  four,  or  five  legs  are  on 
the  ground  at  all  times,  depending  upon  the  desired  speed  and  on  the 
average  terrain  slope  and  roughness. 

2.  If  available,  proximity  sensors  can  be  used  to  maintain  a  specified  foot 
clearance  above  the  terrain  during  the  transfer  phase  of  leg  motion. 
Otherwise,  a  desired  maximum  foot  lift  relative  to  the  body  is  set  by  the 
operator  to  a  value  judged  sufficient  for  the  terrain  being  negotiated. 

3.  The  body  attitude  is  adjusted  so  as  to  permit  effective  use  of  the  limbs 
on  uneven  terrain  without  subjecting  the  operator  to  undue  discomfort 
resulting  from  excessive  body  motion.  In  particular,  the  operator  can 
elect  to  have  the  vehicle  only  partially  adjust  Its  pitch  and  roll  motions 
to  average  terrain  slope. 

4.  The  algorithm  provides  for  an  automatic  reduction  of  body  ground-clearance 
for  enhanced  stability  on  steep  slopes. 
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While  continuing  simulation  studies  and  subsequent  experiments  with  the 
completed  ASV-84  vehicle  will  undoubtedly  produce  improvements  to  the  above 
described  algorithm,  we  feel  that  we  now  have  established  a  satisfactory  base¬ 
line  for  the  conduct  of  such  investigations.  The  operation  of  this  algorithm 
has  been  documented  in  the  form  of  a  videotape  of  a  real-time  vector  graphics 
display  of  simulated  vehicle  motion  with  joystick  control  of  forward,  lateral, 
and  turning  velocities.  A  copy  of  this  videotape  has  been  provided  to  DARPA. 

A  photograph  of  one  frame  of  this  videotape  is  included  in  the  attached 
Appendix  2. 

3.3  Functional  Test  of  Terrain  Mapping  System 

In  November,  1983,  the  terrain  scanner  developed  by  ERIM  was  delivered  to 
Rattelle  Columbus  Laboratories  for  interfacing  to  the  ASV-84  guidance  com¬ 
puter.  Subsequently,  this  system  was  transferred  to  OSU  for  integration  with 
the  vehicle  control  computer.  After  some  initial  problems  relating  to 
electrical  compatibility,  the  entire  system  was  shown  to  function  as  intended 
except  for  the  following  difficulties  noted  with  the  scanner: 

1.  Due  to  excessive  power  consumption  by  the  horizontal  scanning  mirror  drive 
motor,  the  scanner  overheats  after  approximately  fifteen  minutes  of 
operation  at  70°F  ambient  temperature.  This  condition  leads  to  automatic 
shutdown  through  the  action  of  an  integral  thermal  protection  relay. 

2.  The  main  power  relay  sticks  open.  It  has  been  replaced,  and  the  problem 
recurred. 

3.  The  nodding  mirror  requires  more  current  than  anticipated.  On  advice  from 
ERIM,  its  fuse  has  been  replaced  with  one  of  larger  size. 

4.  The  nodding  mirror  frequently  fails  to  initialize  correctly  when  power  is 
turned  on.  When  this  occurs,  the  subsequent  scan  cycle  is  also  incorrect. 


5.  The  scanner  sometimes  produces  incorrect  range  data  due  to  reflections 
from  its  protective  window  occurring  near  the  middle  of  the  azimuth 
and  elevation  scan  angles. 

6.  Frame  scanning  seems  to  begin  prematurely,  before  the  nodding  mirror 
reaches  the  bottom  of  its  travel. 

7.  The  scanner  sends  its  first  line  of  data  in  each  frame  faster  than 
subsequent  lines. 

8.  In  a  laboratory  setting,  we  have  found  that  the  range  measured  by  the 
scanner  is  influenced  by  the  type  of  surface  material  associated  with 
the  objects  scanned. 

All  of  the  above  problems  have  been  communicated  to-ERIM.  It  is  our 
understanding  that  funding  for  a  retrofit  to  correct  these  difficulties  is 
included  in  a  proposal  which  has  been  submitted  by  ERIM  to  DARPA.  At  the  pre¬ 
sent  time,  however,  it  seems  unlikely  that  the  deficlences  noted  can  be 
rectified  within  the  time  frame  of  our  current  contract.  We  therefore  antici¬ 
pate  that  no  ASV-84  system  or  subsystem  tests  requiring  an  operational  scanner 
will  be  undertaken  in  FY  84. 
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Chapter  1 
INTRODUCTION 

1.1  General  Background 

For  more  than  a  decade,  legged  robot  vehicles  have  been  designed  and 
constructed  In  several  Institutions  throughout  the  world  for  a  wide 
range  of  potential  applications,  generally  In  environments  too  hazardous 
for  humans  or  In  those  Inaccessible  to  conventional  wheeled  or  tracked 
vehicles  [1-6].  Some  of  the  possible  applications  for  legged  vehicles 
are  in  the  areas  of  land  and  under-water  exploration,  fire  fighting, 
harvesting  trees  In  the  logging  Industry,  underground  mining,  and 
hazardous  tasks  In  nuclear  power  plants  [5].  The  major  design  objective 
of  legged  robot  vehicles  Is  to  achieve  superior  mobility /walking 
capability,  utilizing  greater  adaptability  to  terr’aln  Irregularities  due 
to  the  fundamentally  different  nature  of  their  Interaction  with  the 
supporting  terrain,  in  comparison  to  wheeled  or  tracked  vehicles  [7]. 

However,  the  advantages  of  legged  robot  vehicles  over  other  types  of 
off-road  vehicles  result  from  their  large  number  of  controllable 
degrees-of- freedom  which  requires  highly  efficient  joint  coordination. 
Controlling  such  a  large  number  of  degrees-of- freedom  is  a  difficult 
task  for  a  human  operator  as  demonstrated  In  [1].  This  problem  has 
led  to  the  development  of  the  computer-controlled  legged  robot  vehicle, 
which  Is  made  possible  by  recent  advances  In  computer  technology. 


Up  to  now,  several  investigations  directed  toward  computer  control 
of  legged  vehicles  have  been  reported  In  the  literature  [4-9].  The 
control  algorithms  developed  to  date  for  locomotion  of  legged  robot 
vehicles  have  most  often  been  based  on  supervisory  control  In  which  the 
human  operator  provides  direction  and  speed  commands  for  the  body,  and  a 
computer  controls  limb  cycling  In  such  a  way  so  as  to  achieve  the 
desired  motion  while  maintaining  stability  [4,8]. 

Prior  to  this  dissertation  work,  researchers  at  The  Ohio  State 
University  have  also  investigated  the  problem  of  computer  control  of 
legged  vehicles.  As  a  result,  basic  control  algorithms  have  been 
developed  to  generate  appropriate  leg  stepping  sequences  and  to  coor¬ 
dinate  body  and  leg  motions  for  the  OSU  Hexapod  which  was  constructed 
for  laboratory  experimentation  of  locomotion  [3,8,10].  Much  of  this 
work  was  concerned  with  level  body  locomotion  over  even  terrain  which  Is 
assumed  relatively  smooth.  The  major  deficiency  of  the  algorithms 
developed  then  Is  their  limited  ability  to  execute  complex  maneuvers 
required  to  traverse  rough  terrain  and  their  lack  of  body  attitude  and 
altitude  control  to  accommodate  terrain  variations.  While  basic 
solutions  to  the  latter  problem  have  been  made  for  the  OSU  Hexapod  using 
force  sensors,  a  vertical  gyroscope,  and  pendulums  [11-13],  the 
algorithms  developed  still  need  improvement  and  modification,  which 
motivates  this  dissertation  work. 

Another  problem  with  the  previous  algorithms  developed  for  the  OSU 
Hexapod  is  that  they  do  not  allow  sufficient  freedom  of  body  motion. 
Three  modes  of  control  have  been  defined- -Cruise  for  forward/backward 
motion.  Side-stepping  for  purely  lateral  movement,  and  Turn-In-Place  In 
which  the  body  Is  rotated  about  its  center  of  gravity  with  no  lateral  or 
forward/backward  movement.  The  boty  velocity  component  values  are 


obtained  from  a  human  operator  through  a  three-axis  joystick.  However, 
restrictions  on  these  components  are  made  In  each  of  the  control  modes 
so  that  legs  will  be  maintained  within  their  kinematic  limits  and 
vehicle  stability  is  ensured.  It  is  desirable  to  remove  the 
restrictions  placed  on  body  velocity  and  to  integrate  the  three  modes  of 
operation  to  obtain  omnidirectional  control,  and  this  also  motivates 
this  dissertation  work. 

The  objective  of  this  work,  then,  is  to  develop  control  algorithms 
to  combine  the  three  different  modes  of  operation  defined  for  the  OSU 
Hexapod  [8]  to  provide  for  .omnidirectional  control  and  to  extend  and 
modify  the  previous  motion  planning  algorithms  to  accomplish  greater 
adaptability  to  uneven  terrain.  The  algorithms  are  developed  through 
the  use  of  computer  simulation  on  the  PDP-11/70  minicomputer.  A  graphics 
display  device  (HP  1350A)  is  used  extensively  for  motion  display 
and  various  terrain  conditions  may  be  generated  interactively.  The 
result  is  that  an  approach  to  computer-aided  design  of  motion  planning 
algorithms  has  been  developed.  Since  the  algorithms  developed  in  this 
work  will  be  applied  to  a  hexapod  robot  vehicle,  named  the  Adaptive 
Suspension  Vehicle  (ASV),  which  Is  currently  under  construction  at  The 
Ohio  State  University,  the  vehicle  model  for  the  simulation  Is  chosen 
from  it. 

The  major  problems  dealt  with  in  this  simulation  work  may  be 
partitioned  Into  the  following  three  areas: 

1)  In  order  to  investigate  locomotion  over  uneven  terrain,  a 
definition  of  the  terrain  model  over  which  the  vehicle  moves  is 
required.  The  terrraln  model  should  be  relatively  simple  to  generate 
but,  on  the  other  hand,  should  present  somewhat  realistic  terrain 
conditions  to  vehicle  motion. 
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2)  In  order  to  Increase  maneuverabl 1 1 ty  of  the  vehicle,  the 
limitations  Imposed  on  the  body  motion  trajectory  by  the  previous 
control  algorithms  should  be  removed.  In  particular,  omnidirectional 
control  through  a  three-axis  joystick  should  be  provided.  At  the  same 
time,  stability  of  the  vehicle  should  be  maintained  with  proper  leg 
stepping  sequences. 

3)  The  final  problem  investigated  involves  the  development  of 
control  algorithms  for  the  Close  Maneuvering  mode  of  operation  for  the 
Adaptive  Suspension  Vehicle  (ASV)  [14].  This  mode  of  operation  requires 
proximity  and  contact  sensors  for  local  terrain  sensing  at  each  foot. 

In  order  to  accommodate  terrain  irregularities,  the  body  attitude  and 
altitude  should  be  appropriately  controlled.  However,  this  body 
regulation  tends  to  reduce  stability  of  the  vehicle  over  sloped  terrain. 
Therefore,  approaches  to  Increase  stability  should  be  investigated. 
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1.2  Organization 

A  brief  review  of  the  previous  research  on  legged  vehicle  locomo¬ 
tion  directed  to  develop  control  algorithms  Is  given  In  Chapter  2.  This 
chapter  includes  previous  work  on  gaits,  a  discussion  of  the  operational 
modes  for  the  ASV,  supervisory  control  schemes  adopted  for  the  OSU 
Hexapod,  and  feedback  control  strategies  presently  employed  for 
low-level  servo  control. 

In  Chapter  3,  terrain  and  vehicle  models  used  In  the  simulation 
are  presented  In  detail.  Also,  the  computer-aided  design  concept  for 
the  development  of  control  algorithms  Is  described. 

Chapter  4  details  an  algorithm  to  integrate  the  three  joystick 
control  modes  previously  defined  for  the  OSU  Hexapod  [8]  in  order  to 
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provide  omnidirectional  control.  Periodic  gaits  are  used  and  much  of 
the  chapter  presents  a  new  method  for  choosing  an  optimal  cycle  period. 
The  concept  of  the  constrained  working  volume  is  also  presented. 

Chapter  5  describes  algorithms  that  implement  the  Close  Maneuvering 
mode  of  operation  for  the  ASV  for  uneven  terrain.  Details  of  the  body 
and  leg  motion  planning  algorithms  are  presented  along  with  discussion 
concerning  the  dynamic  changing  of  the  size  of  the  constrained  working 
volume  to  increase  the  stability. 

Chapter  6  covers  the  overall  software  structure  of  the  control 
program  and  the  graphics  routines  along  with  the  data  base  for  the 
terrain  and  vehicle  models.  Finally,  different  evaluations  of  the 
control  algorithms  developed  in  this  work  are  discussed. 

Finally,  Chapter  7  summarizes  the  contributions  made  in  this 
dissertation  work  with  a  discussion  of  possible  further  research.  An 
appendix  is  included  which  contains  a  listing  of  the  control  programs 
which  were  written  in  PASCAL  on  the  PDP-11/70  minicomputer. 


Chapter  2 

REVIEW  OF  EXISTING  CONTROL  ALGORITHMS 

2.1  Introduction 

This  chapter  attempts  to  give  a  brief  overview  of  previous  research 
directed  to  develop  the  control  algorithms  for  legged  vehicle 
locomotion.  A  major  thrust  of  much  of  the  early  work  was  to  provide  a 
theoretical  formalization  of  gaits  of  human  beings  and  animals,  and 
this  has  been  well  established  In  the  literature  [15-17]  and  will  be 
reviewed  here.  On  the  other  hand,  control  algorithms  to  Implement  the 
gaits  In  legged  vehicle  locomotion  are  still  under  development  and  the 
latest  work  will  also  be  outlined. 

Over  the  past  two  decades,  a  number  of  walking  machines  have  been 
constructed  to  Implement  gaits  and  to  develop  control  algorithms  In  the 
laboratory  environment  with  coordination  of  multiple  legs.  Also,  some 
special  machines  which  utilize  legged  locomotion  principles  were 
commercially  built.  Typical  examples  of  the  former  are  the  G.E. 
Quadruped  Transporter  [1],  'Phoney  Pony'  [2],  the  OSU  Hexapod  [3],  and 
the  Moscow  State  University  Hexapod  [4].  For  the  latter  case,  there 
are  the  Menzi  Muck  Climbing  Hoe  [18],  the  Kaiser  Spyder  [19]  and  ODEX  1 
[5]. 

Through  experimentation  with  the  above  walking  machines,  several 
approaches  to  control  of  legged  locomotion  systems  have  been  explored. 
These  Include  computer  control  of  the  joints,  supervisory  control  for 


the  man-machine  interface,  and  feedback  control  at  the  servomechani sm 
level  to  provide  energy  efficiency,  load  balancing  and  active 
compliance. 

All  of  the  above  control  concepts  have  been  applied  to  the  control 
of  the  OSU  Hexapod  in  both  software  and  hardware  implementation.  It  was 
built  in  1976  at  The  Ohio  State  University  in  order  to  allow  laboratory 
experimentation  of  legged  vehicle  locomotion.  This  vehicle  is 
electrically  powered  and  each  leg  possesses  three  degrees  of  freedom 
with  three  revolute  joints.  An  insect-type  of  leg  kinematics  is 
employed  for  the  leg  structure.  The  control  algorithms  developed  for 
the  OSU  Hexapod  are  well  documented  in  [11]. 

Based  on  experience  with  the  OSU  Hexapod,  another  six-legged 
vehicle,  called  the  Adaptive  Suspension  Vehicle  (ASV),  is  currently 
under  construction  at  The  Ohio  State  University.  The  ASV  will  be 
roughly  the  same  size  as  the  G.E.  Quadruped  and  hydraulically  powered. 
The  leg  structure  will  be  based  on  a  mammalian  configuration  with 
pantograph  coordination  of  joint  motions  [20]  in  order  to  decouple 
actuator  loads.  The  present  model  for  the  ASV  is  used  In  the  simulation 
study  In  this  dissertation  work. 

In  Section  2.2,  some  theoretical  background  material  for  gaits  is 
presented  and  provides  a  basis  for  much  of  the  dissertation  work. 

Several  operational  modes  for  the  ASV  have  been  proposed  by  McGhee  et 
al.  [14],  depending  on  the  terrain  conditions,  fuel  economy,  maneuver¬ 
ability  of  the  vehicle  or  vehicle  sensors  used,  and  these  are  described 
In  Section  2.3.  In  Section  2.4,  two  different  motion  planning  control 
schemes,  based  on  the  supervisory  control  concept,  are  discussed. 

Finally  In  Section  2.5,  two  of  the  feedback  control  schemes  developed  at 
the  servomechanism  level  for  walking  machines  are  briefly  reviewed. 


2.2  Gaits 

Due  to  the  discrete  aspects  of  legged  locomotion,  the  natural  and 
theoretically  possible  patterns  of  leg-lifting  and  leg-placing  events, 
called  gaits,  used  by  human  beings  and  animals  have  been  Investigated 
since  the  first  classification  of  gaits  by  Muybridge  [15,16].  In  the 
1960's,  Hildebrand  [17]  formulated  the  definition  of  a  gait  and 
developed  a  more  quantitative  approach  to  the  classification  of 
symmetrical  gaits. 

Later,  McGhee  [21]  established  a  general  mathematical  theory  of 
legged  locomotion  from  the  point  of  view  of  finite  state  machine 
theory  by  regarding  each  leg  as  a  two- state  (1  and  0)  sequential 
machine.  McGhee  then  Introduced  a  gait  matrix,  G,  which  Is  formally 
defined  as  a  k-column  matrix  whose  successive  rows  are  binary  k- tuples 
corresponding  to  the  the  successive  states  of  a  particular  gait  of 
k-1 egged  locomotion.  A  zero  entry  In  column  j  Indicates  that  leg  j  is 
In  contact  with  the  ground  (support  phase)  while  a  1  entry  means  that 
It  is  not  (transfer  phase). 

McGhee  and  Jain  [22]  Introduced  another  mathematical  description  of 
a  gait,  the  "event  sequence".  If  the  legs  of  the  vehicle  are  numbered, 
l,2,...,k,  then  the  event  of  placing  leg  1  Is  denoted  event  i,  while 
lifting  of  leg  1  Is  arbitrarily  denoted  event  1+k.  The  event  sequence 
Is  further  employed  In  this  section  to  describe  gaits. 

2.2.1  Stability 

During  legged  vehicle  locomotion.  It  Is  essential  that  the  vehicle 
should  not  fall  over.  This  problem  led  to  stability  studies  on  legged 
vehicle  locomotion  by  many  researchers. 


The  stability  of  a  vehicle  may  be  measured  either  statically  or 
dynamically  during  locomotion.  The  static  stability  concept  is  employed 
in  this  work,  since  it  provides  a  simpler  basis  for  the  control.  The 
test  of  static  stability  involves  determining  whether  the  vertical 
projection  of  the  center  of  gravity  of  the  vehicle  onto  the  supporting 
surface  is  contained  within  a  polygon  whose  vertices  are  defined  by  the 
feet  in  contact  with  the  ground.  This  notion  was  formally  defined  in 
[23]  through  the  following: 

Definition  1:  The  support  pattern  associated  with  a  given  support 
state  [7]  is  the  convex  hull  (minimum  area  convex  polygon)  of  the 
point  set  in  a  horizontal  plane  which  contains  the  vertical  projections 
of  the  feet  of  all  supporting  legs. 

There  can,  however,  exist  a  large  number  of  statically  stable 
gaits  due  to  the  combinatorial  aspects  of  the  gait  selection  problem. 

In  order  to  choose  optimal  gaits  in  the  sense  of  static  stability,  the 
following  measurement  of  gait  stability  Is  defined  [23]. 

Def i ni ti on  2 :  The  longitudinal  stability  margin  i s  the  shortest 
distance  over  an  entire  cycle  of  locomotion  from  the  projection  of  the 
vehicle  center  of  gravity  onto  the  support  plane  to  the  edge  of  the 
support  pattern  as  measured  in  the  direction  of  travel. 

The  longitudinal  stability  margin  is  defined  only  for  straight- 
line  locomotion.  When  motions  of  the  vehicle  are  arbitrary,  the  above 
definition  must  be  expanded.  Thus,  the  stability  margin  may  be  defined 
for  general  periodic  gaits  to  Include  the  case  of  arbitrary  locomotion. 

Definition  3:  The  stability  margin  Is  the  shortest  distance  over 
an  entire  cycle  of  locomotion  from  the  projection  of  the  vehicle  center 
of  gravity  onto  the  horizontal  plane  to  the  edge  of  the  support 
pattern . 


For  hexapod  vehicles,  Bessonov  and  Umnov  [24]  found  a  class  of 
optimally  stable  gaits,  called  wave  gaits,  for  straight-line  locomotion 
by  using  the  mini max  longitudinal  stability  criterion  given  in 
Definition  2.  Optimal  wave  gaits  have  the  property  that  the  stepping 
events  on  each  side  of  the  vehicle  move  from  the  rear  to  the  front  of 
the  body  for  forward  motion  (forward  wave  gaits),  while  they  progress 
from  the  front  to  the  rear  of  the  body  for  backward  motion  (backward 
wave  gaits).  The  terms  "forward"  and  “backward"  are  designated  with 
respect  to  the  head  of  the  vehicle  (fixed).  If  there  is  no  distinction 
between  the  head  and  tail  of  the  vehicle,  the  forward  and  backward  wave 
gaits  represent  the  same  leg  sequences  with  respect  to  the  direction  of 
travel  and  are  both  optimal.  In  other  words,  the  optimal  gaits  are 
symmetric. 

2.2.2  Periodic  Gaits 

If  every  limb  of  a  legged  vehicle  or  animal  operates  with  the  same 
cycle  time,  then  the  gait  Is  said  to  be  periodic  [25],  The  following 
paragraphs  briefly  discuss  periodic  gaits  and  also  define  two  particular 
classes  of  periodic  gaits  used  in  artificial  legged  locomotion  systems: 
optimal  wave  gaits  and  Follow-The-Leader  gaits  [26]. 

The  following  definitions  for  straight-line  motion  were  Introduced 
by  McGhee  and  Frank  [23]. 

Definition  4:  The  period,  t.  Is  the  time  required  for  one  complete 
locomotion  cycle  of  the  gait. 

Definition  5:  The  stride  length,  \t  of  a  gait  is  the  distance  by 
which  the  center  of  gravity  of  the  locomotion  system  is  translated 
during  one  complete  locomotion  cycle. 
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Definition  6:  The  duty  factor,  g,  is  the  fraction  of  a  locomotion 
cycle  that  each  leg  spends  in  contact  with  the  supporting  surface. 

Definition  7:  The  relative  phase,  $ ,  is  the  fraction  of  a 
locomotion  cycle  by  which  the  contact  of  leg  i  with  the  supporting 
surface  lags  behind  the  contact  of  leg  1  (left  front  leg). 

Bessonov  and  Umnov  [24]  have  shown  that  optimal  wave  gaits  for  a 
hexapod  may  be  described  through  the  following  equations: 

^  8  0  4>ga2g-l  1  >  0  >  0.5  .  (2.1) 

Note  that  legs  on  the  left  side  of  a  hexapod  be  numbered  from  front  to 
rear  as  1,3,5  successively  and  those  on  the  right  side  in  the  same  order 
as  2,4,6.  Since  these  gaits  are  symmetric  [17,21],  the  relative  phases 
of  the  legs  of  any  right-left  pair  are  exactly  half  a  cycle  out  of  phase 
with  each  other.  The  event  sequence  diagram  for  optimal  wave  gaits  with 
g  *  5/6  is  shown  in  Figure  2.1. 

In  addition  to  the  above  definitions,  Orin  [8]  provided  the 
following  definition  for  implementation  of  periodic  gaits. 

Definition  8:  The  kinematic  cycle  phase,  $,  is  the  distance  by 
which  the  center  of  gravity  of  the  locomotion  system  has  translated 
since  the  last  placement  of  leg  1,  normalized  to  the  stride  length. 


The  relation  between  the  kinematic  cycle  phase,  4,  and  the  period, 
x,  is  given  in  [8]: 

<P  a  f  —  dt. 

T 


(2.2) 


By  di f ferenti atl ng  both  sides  of  Eq.  (2.2),  with  respect  to  time,  the 
following  Is  obtained: 


4L  s  I 

dt  t 


(2.3) 


(2.4) 


Eq.  (2.4)  may  be  approximated  by  replacing  dt  with  At  and  <4  with  A4>, 
respectively,  such  that 


Eq.  (2.5)  provides  an  approximate  means  to  compute  the  period  and 
will  be  discussed  further  In  Chapter  4.  Note  that  once  the  period,  t. 

Is  defined,  the  value  of  4  Is  either  Increasing  or  decreasing  according 
to  the  sign  of  t  In  Eq.  (2.3). 

The  leg  placing  and  lifting  times  may  be  determined  by  the  use  of 
the  kinematic  cycle  phase,  $,  and  the  relative  phase,  4^.  In  Figure 
2.1,  $  moves  clockwise  along  the  circle,  when  Increasing  and  moves 
counterclockwise,  when  decreasing.  When  4)  moves  clockwise,  the  leg 
placing  sequence  for  one  locomotion  cycle  is  leg  6,  4,  2,  5,  3,  and  1, 
which  implements  the  forward  wave  gait.  On  the  other  hand,  when  4*  moves 
counterclockwise,  the  leg  placing  sequence  Is  leg  1,  3,  5,  2,  4,  and  6, 
which  Implements  the  backward  wave  gait. 

From  the  above  discussion,  the  relationship  between  4>  and 
determines  the  state  or  phase  for  leg  1:  transfer  phase  or  support 
phase.  Let  the  leg  phase  variable  of  leg  1,  4»L1>  be  defined  as 


♦.  1  *  U  “  4^mod  1* 


(2.6) 


support  phase 
transfer  phase. 


(2.7) 

(2.8) 


o  <  *Li  <6 
e  <  ♦Li  <  i 


During  the  transfer  phase  of  leg  1,  ^  changes  from  $  for  lift-off 
to  1  for  touchdown  for  Increasing  4>^  and  vice  versa  for  decreasing 
values.  Accordingly,  a  new  phase  variable,  4.^,  for  the  transfer  leg  as 
normalized  to  the  total  transfer  time  may  be  computed  as  follows: 


1  -  0 


< 


for  t  >  0 


for  t  <  0. 


(2.9) 


Thus,  when  the  leg  lifts  off,  ^  *  0,  and  when  the  leg  touches  down, 
*T1  *  lm 

In  a  similar  manner,  $L1  changes  from  0  for  touchdown  to  b  for 
lift-off  during  the  support  phase  when  Increasing  and  vice  versa  when 
decreasing.  Another  phase  varlble,  4^,  for  the  support  leg  as 
normalized  to  the  total  support  time  may  be  obtained  as  follows: 


for  t  >  0 


for  x  <  0 


(2.10) 


so  that  always  changes  from  0  for  touchdown  to  1  for  lift-off. 

The  graphical  representation  of  the  above  phase  variables  Is  shown 
In  Figure  2.2.  While  the  example  used  In  the  above  discussion  was  for 


a  wave  gait,  other  types  of  periodic  gaits  may  be  easily  Implemented 
by  altering  the  relative  phase  relationships  of  the  legs. 

Recently,  another  class  of  periodic  gaits,  called  Follow-The-Leader 
(FTL)  gaits,  have  been  Investigated  [26].  In  FTL  gaits,  the  middle  and 
rear  legs  use  the  same  footholds  as  are  used  by  the  front  leg  on  the 
same  side  of  the  body.  For  example,  the  placing  sequence  of  one 
locomotion  cycle,  1-3-5-2-4-6,  possesses  the  FTL  property.  This 
sequence  Is  the  same  as  that  for  the  backward  wave  gait  as  mentioned 
before.  However,  the  stability  of  a  FTL  gait  Is  not  optimal,  because 
the  vehicle  always  moves  forward  In  the  FTL  mode.  In  this  sense,  FTL 
gaits  and  optimal  wave  gaits  are  two  different  families  of  gaits  whose 
Intersection  is  empty  [26]. 

2.2.3  Free  Gaits 

As  discussed  in  the  previous  section,  periodic  gaits  are  totally 
governed  by  the  fixed  relative  phase  relationships  of  the  legs.  In 
other  words,  once  the  relative  phases  are  defined,  the  leg  lifting  and 
placing  events  are  fixed  for  one  cycle  of  locomotion  regardless  of  the 
terrain  conditions.  This  results  In  control  algorithms  which  are 
relatively  simple  to  Implement.  Also,  for  locomotion  over  level 
terrain,  periodic  gaits  may  be  efficient  and  optimal. 

However,  as  the  degree  of  terrain  1 rregul arl ti es  Increases, 
periodic  gaits  mqy  lose  such  efficiency  and  optimality.  Since  free 
gaits  have  no  fixed  relative  phase  relationships  among  the  legs,  there 
exists  an  Infinite  number  of  possible  gaits.  This  allows  the  free  gait 
control  algorithms  to  choose  an  optimal  gait  for  locomotion  over  rough 
terrain  with  great  flexibility.  Implementation  of  the  control 


algorithms,  however,  may  be  more  complicated  than  that  for  periodic 
gaits. 

The  complete  theoretical  formalization  for  the  free  gaits  has  not 
yet  been  established,  and  the  algorithms  developed  are  rather  heuristic 
as  compared  to  those  for  periodic  gaits.  Kugushev  and  Jaroshevsklj  [27] 
have  suggested  that  the  mathematical  approach  to  periodic  gaits  for 
straight-line  locomotion  may  be  extended  to  include  the  nonperiodic 
gaits,  i.e.,  free  gaits.  In  [27],  the  problem  for  free  gaits  is 
partially  formalized  in  such  a  way  that  a  trajectory  is  specified  in 
advance  for  the  motion  of  the  center  of  gravity  of  a  legged  system  over 
a  given  terrain  containing  certain  regions  which  are  unsuitable  for - 
support. 

Later,  McGhee  and  Iswandhi  [7]  extended  the  work  of  Kugushev  and 
Jaroshevsklj  to  complete  the  formalization  of  this  problem  and 
developed  a  heuristic  algorithm.  Specifically,  In  [7],  the  terrain 
was  divided  into  discrete  cells,  each  about  the  size  of  a  footprint. 

Each  cell  was  designated  as  permi tted  (suitable  for  weight  bearing),  or 
forbidden  to  simulate  the  terrain  Irregularities.  The  control  algorithm 
develpoed  in  this  work  was  based  on  the  kinematic  margin  of  a  foothold 
which  relates  to  how  long  a  foot  can  be  on  a  given  cell  before  the  leg 
reaches  Its  kinematic  limit  [7].  If  the  vehicle  remains  stable,  legs 
are  lifted  so  as  to  maximize  the  minimum  value  of  the  kinematic  margin 
over  all  supporting  legs.  Otherwise,  the  algorfthm  corrects  the 
unstable  situation  by  placing  a  leg  In  the  air  when  this  is  possible. 
Whenever  a  leg  Is  placed,  among  those  legs  which  make  the  vehicle 
stable,  the  one  with  the  largest  kinematic  margin  is  utilized.  Thus, 
the  basic  criteria  for  leg  sequencing  in  this  algorithm  are  the 


kinematic  margin  of  each  leg  and  the  stability  margin  in  the  support 
pattern. 

The  above  algorithm  was  further  refined  by  Kwak  [28],  who  employed 
the  same  criteria  except  that  the  kinematic  margin  is  measured  in  time 
rather  than  in  distance.  In  his  work,  improvements  to  the  basic 
algorithms  suggested  by  McGhee  and  Iswandhi  [7]  were  implemented  in 
software  on  the  PDP-11/70  minicomputer  using  the  HP  1350A  graphics 
device  for  display  of  the  vehicle  and  the  terrain  cells. 

Recently,  Patterson  [29]  proposed  the  control  algorithm  for  free 
gaits  in  coorporation  with  the  guidance  algorithm  using  the  terrain 
scanner  whfch  provides  a  terrain  preview  of  average  terrain  slope  using 
ultrasonic  or  laser  signal.  In  this  algorithm,  the  desired 
accelerations  for  the  body  motion  of  the  vehicle  are  generated  based  on 
the  operator's  velocity  commands  and  the  current  vehicle  velocity.  If 
the  desired  accelerations  cause  that  the  motion  of  the  vehicle  is 
unstable  or  the  desired  foothold  of  the  leg  is  out  of  its  kinematic 
limit,  then  the  deceleration  plan  [29]  Is  Invoked  to  slow  down  the 
vehicle  speed  or  to  allow  the  vehicle  to  be  brought  to  a  halt  while 
maintaining  stability. 

2.3  Operational  Modes  of  the  ASY 

Control  of  vehicle  locomotion  strongly  depends  on  both  the  terrain 
conditions  for  the  desired  motion  of  the  vehicle  and  the  sensors  used. 
The  sensors  used  for  vehicle  locomotion  are  force  sensors,  proximity 
sensors,  the  terrain  scanner,  etc.  Especially,  the  use  of  the  terrain 
scanner  affects  the  operational  modes  over  rough  terrain.  Presently, 
five  operational  modes  [14]  are  considered  to  be  Implemented  In  control 


of  the  ASV--Cruise  Mode,  Terrain  Following  Mode,  Dash  Mode,  Close 
Maneuvering  Mode,  and  Precision  Footing  Mode. 

The  Cruise  Mode  is  for  locomotion  over  relatively  smooth  terrain 
with  minimization  of  fuel  consumption.  In  this  mode,  the  body  altitude 
and  attitude  of  the  vehicle  could  be  maintained  at  the  desired  values  by 
the  computer  using  terrain  and  inertial  sensors.  The  body  crab  angle, 
which  is  defined  as  an  angle  between  the  heading  of  the  vehicle  and  the 
instantaneous  body  velocity  vector,  could  be  limited  to  a  relatively 
small  value  and  minimum  turning  radius  could  be  of  the  order  of  several 
body  lengths.  That  reduces  manueverabi 1 1 ty  somewhat,  but  the  optimum 
speed  of  the  vehicle  can  be  achieved  in  the  sense  of  fuel  economy. 

In  order  to  increase  maneuverability  for  the  locomotion  over  rough 
terrain,  the  limitations  Imposed  on  the  Cruise  Mode  may  be  removed. 

The  Terrain  Following  Mode  is  essentially  for  forward  locomotion  over 
rough  terrain  with  a  slower  speed  than  that  in  the  cruise  mode  and 
moderate  fuel  consumption.  In  this  mode,  all  the  vehicle  sensors  can 
be  used  such  as  the  terrain  scanner,  proximity  sensors,  force  sensors, 
etc.  The  terrain  scanner  can  provide  a  terrain  preview  to  predict 
foothold  locations  and  terrain  slope  information  for  body  attitude 
and  altitude  regulation.  The  proximity  sensors  are  used  for  local 
control  of  foot  positions,  while  the  force  sensors  can  be  used  for  force 
feedback  control  to  achieve  active  compliance  [30].  Hence,  automatic 
body  regulation  can  be  accomplished  by  using  sensor  Information. 

The  Dash  Mode  Is  desired  when  speed  of  the  vehicle  Is  more  Important 
than  maneuverability  or  fuel  efficiency.  In  this  mode,  limitations  are 
Imposed  on  both  crab  angles  and  the  body  turning  radius  to  obtain 
maximum  foot  velocities. 


For  very  difficult  terrain  where  human  intelligence  can  be  more 
appropriate  than  the  sensors  to  choose  proper  foothold  positions 
without  concern  for  speed,  the  Individual  legs  may  be  controlled  by 
the  operator  by  means  of  joystick  or  keyboard  commands.  Such  a  control 
mode  is  termed  as  "precision  footing".  In  this  mode,  feedback  of  foot 
position  relative  to  terrain  is  done  by  operator  vision.  The  proximity 
sensors  are  used  to  control  the  foot  altitude. 

Since  the  terrain  scanner  is  only  useful  for  the  forward  locomotion, 
it  can  not  be  used  for  backward,  sideway,  or  turning  motion.  The  Close 
Maneuvering  Mode  of  operation  which  requires  an  arbitrary  motion: 
a  combination  of  forward,  backward,  sideway,  and  turning  motions,  is 
for  locomotion  over  reasonably  rough  terrain  without  terrain  preview. 
Thus,  speed  of  vehicle  is  relatively  slow  compared  with  that  in  the 
Terrain  Following  Mode.  This  Is  a  three-axis  control  mode  In  which 
the  body  turning  radius  and  crab  angles  are  not  restricted  to  achieve 
high  degree  of  maneuverability.  Body  attitude  and  altitude  are 
automatically  regulated  to  accomplish  the  terrain  following  locomotion 
through  the  proximity  or  force  sensors. 

2.4  Supervisory  Control  of  the  OSU  Hexapod 

The  advantages  of  legged  vehicles  over  wheeled  or  tracked  vehicles 
result  from  their  greater  number  of  controllable  degrees  of  freedom. 
However,  the  manual  control  of  such  a  large  number  of  degrees  of  freedom 
Is  an  exhausting  task  for  the  human  operator  to  endure  as  revealed  in 
the  test  of  the  G.  E.  Quadruped  Vehicle. 

In  order  to  relieve  a  human  operator  of  such  a  complex  task  as  the 
manual  coordination  of  limb  motions,  an  appropriate  control  strategy  for 
the  man-machine  interface  was  developed  [8],  which  Is  called  supervisory 


control.  The  basic  hierarchical  structure  of  the  supervisory  control 
scheme  employed  In  the  control  of  the  present  OSU  Hexapod  Is  shown  In 
Figure  2.3  [8]. 

2.4.1  Three- axis  Control 

In  this  supervisory  control  scheme,  the  operator  provides 
longitudinal  velocity,  lateral  velocity,  and  yaw  turning  rate  with 
respect  to  the  body  to  the  motion  planning  block.  These  velocities  and 
rates  can  be  generated  by  a  three-axis  joystick  or  a  similar  device. 

For  the  OSU  Hexapod  control,  the  three-axis  control  mode  has  been 
implemented  for  locomotion  over  flat  level  terrain  using  a  three-axis 
joystick  [8].  In  this  Implementation,  the  vehicle  operation  was  divided 
Into  three  modes  called  Cruise  mode.  Turn-in-place  mode  and 
Side-stepping  mode  according  to  the  body  velocity  [8].  This  was  done  in 
order  to  avoid  complexity  In  leg-trajectory  planning  for  unrestricted 
•three-axis  control.  In  all  three  modes,  the  body  of  the  vehicle  Is 
assumed  to  remain  at  a  constant  height  and  parallel  to  the  plane  of 
support. 

In  Cruise  mode,  the  minimum  turning  radius  of  the  motion  trajectory 
of  the  center  of  gravity  of  the  vehicle  and  Its  crab  angle  Is  limited  so 
that  this  mode  Is  appropriate  for  cruising  forward  or  backward  with 
small  components  of  slde-to-slde  velocity  or  turning  rate. 

Turn-In-place  mode  Is  for  the  turning  motion  of  the  vehicle  In  place 
about  Its  center  of  gravity  with  the  other  two  components  of  vehicle 
velocity  forced  to  zero.  Side-stepping  mode  Is  for  the  lateral  motion 
of  the  vehicle  without  changing  Its  heading,  while  the  other  two 
components  of  vehicle  velocity  are  forced  to  zero. 


For  the  locomotion  over  even  terrain,  the  other  three  components  of 
the  body  velocity:  up/down  velocity,  roll  rate,  and  pitch  rate,  are  set 
to  zero.  Hence,  the  body  height  remains  constant  above  the  terrain  and 
there  Is  no  attitude  and  altitude  control  during  locomotion. 

Based  on  the  above  control  scheme,  an  algorithm  was  developed  by 
Chang  [12]  for  locomotion  of  the  OSU  Hexapod  over  uneven  terrain.  In 
this  algorithm,  the  body  roll  and  pitch  rates  are  generated  in  the 
motion  planning  block  for  the  automatic  body  attitude  control  so  that 
the  body  may  be  parallel  to  the  estimated  support  plane  obtained  from 
the  present  support  points  through  linear  regression.  Also,  the 
up/down  velocity  of  the  body  Is  computed  in  the  motion  planning  block 
In  order  to  keep  the  body  height  constant  above  the  estimated  support 
plane.  The  estimated  support  plane  is  updated  whenever  one  of  the 
transfer  legs  touches  ground. 

2.4.2  Follow-The-Leader  Control 

For  the  full  automation  of  the  walking  machine,  it  is  necessary  to 
raise  the  level  of  the  man-machine  communication  scheme  from  simple 
steering  of  the  body  to  that  of  optical  designation  of  footholds. 

Toward  this  end,  the  Follow-The-Leader  (FTL)  control  has  been  employed 
for  locomotion  of  the  OSU  Hexapod  vehicle  over  rough  terrain  [26].  In 
this  mode  of  operation,  the  ranging  system,  which  provides  a  terrain 
Information  only  for  front- leg  footholds,  permits  the  full  utilization 
of  human  Intelligence  In  selecting  the  footholds.  The  human  operator 
specifies  the  desired  footholds  for  the  front  pair  of  legs  and  the 
successive  legs  of  each  side  of  the  body  step  on  the  same  points  as  used 
by  the  front  legs. 


First,  the  candidate  footholds  are  selected  by  the  human  operator 
through  the  optical  sensing  device.  Then  the  computer  of  the  vehicle 
checks  whether  or  not  they  are  Inside  the  kinematic  limit  of  the  leg. 

If  they  are  Inside  the  kinematic  limit,  they  are  accepted  by  the 
vehicle.  Otherwise,  they  are  rejected.  When  the  candidate  footholds 
are  accepted,  then  the  motion  planning  block  initiates  the  body  motion 
planning  by  determining  the  desired  vehicle  velocity  vector  and  the 
turning  rate  so  that  the  vehicle  moves  the  desired  direction,  which  Is, 
in  contrast,  provided  by  the  human  operator  In  the  three-axis  control 
mode.  When  the  candidate  footholds  are  rejected,  the  computer  requests 
another  selection  from  the  human  operator  until  an  acceptable  foothold 
is  found. 

One  of  the  important  problems  relating  to  the  FTL  control  algorithm 
Is  how  to  determine  the  vehicle  center  of  gravity  for  the  next  cycle  of 
body  motion.  Tsai  [31]  developed  a  heuristic  algorithm  to  find  such  an 
optimal  point  for  the  vehicle  center  of  gravity  by  maximizing  the 
stability  margin.  The  motion  planning  algorithm  for  the  FTL  control  is 
described  in  detail  In  [31],  which  was  Implemented  and  tested  for  the 
OSU  Hexapod  over  rough  terrain  in  a  laboratory  environment. 

2.5  Feedback  Control 

As  shown  In  Figure  2.3,  the  body  motions  are  finally  executed  by  the 
motion  execution  block  in  the  servomechanism  level.  Since  this  level 
Involves  the  full  dynamic  complexity  of  the  vehicle/ terrain  system.  It 
Is  necessary  to  develop  an  appropriate  control  strategy  to  obtain  the  - 
desired  joint  motions.  Various  control  concepts  have  been  developed  for 
walking  machines.  The  following  paragraphs  describe  two  of  them  which 


are  presently  implemented  for  control  of  the  existing  legs  at  the  Ohio 
State  University. 

2.5.1  Jacobian  Control 

Jacobian  control  is  based  on  resolved  rate  at  the  foot  so  that  the 
control  is  made  in  Cartesian  coordinates.  Also,  this  control  scheme 
incorporates  both  force  and  position  feedback.  The  Jacobian  control 
law  is  given  by  the  following  equation  as 

lc  *  J_1[xp  +  Kp(xp  -  x*)  +  Kp(Fp  -  F^)]  (2.11) 

where  xp  Is  the  desired  foot  position  derived  by  the  desired  rate,  x^  Is 
the  actual  foot  position,  Fp  Is  the  desired  foot  force,  and  is  the 
actual  foot  force.  The  values  of  the  elements  of  the  gain  matrices  Kp 
and  Kp  determine  the  accomodation  [32]  of  force  errors  by  position 
errors  and,  for  diagonal  matrices,  lead  to  the  concept  of  active 
compliance  [30].  The  Jacobian  control  structure  with  compliance  servo 
for  the  OSU  Hexapod  is  shown  In  Figure  2.4  [11].  In  this  figure, 
denotes  the  vector  of  the  joint  actuator  Input  voltages  and  Ff  denotes 
the  foot  force  In  the  foot  coordinate  system. 

Recently,  based  on  the  Jacobian  control  structure,  force  feedback 
control  for  active  compllnce  using  force  sensors  and  attitude  control 
using  Inertial  sensors  have  been  Implemented  and  experimentally  . 
evaluated  for  the  OSU  Hexapod  [11].  In  this  work,  each  of  the  feet  has 
been  equipped  with  two  semiconductor  strain  gauges  to  measure  lateral 
forces  and  a  plezoelectronlc  load  cell  to  measure  vertical  forces.  A 
vertical  gyroscope  and  pendulums  as  the  attitude  sensors  have  also  been 
used.  The  desired  foot  force  Fp  was  computed  from  the  conditions  such 
that  the  vehicle  body  maintains  static  equilibrium.  However,  the 


conditions  resulted  In  the  underspecified  set  of  linear  equations.  The 
optimal  solution  for  Fg  was  obtained  as  a  pseudo-inverse  solution  to 
these  equations  using  the  least  squares  of  force  criterion  [11]. 

The  force  feedback  control  law  for  vertical  active  compliance  under 
ideal  rate  servo  assumption  is  given  by  [13] 

z/^  =  zg  +  kp(zg  -  z^)  +  kp(Fg  -  F/[)  (2.12) 

where  zA  and  zg  are  the  actual  and  desired  vertical  foot  velocities;  z A 
and  zg  are  the  actual  and  desired  vertical  positions;  Fg  and  FA  are 
actual  and  desired  vertical  forces;  kp  and  kp  are  gains.  Eq.  (2.10) 
results  in  the  spring-damper  system  when  the  spring  stiffness,  ks,  and 
the  damper  coefficient,  a,  are  chosen  as  [30] 

ks  =  kp/kp  (2.13) 

and 

a  *  1/kp.  (2.14) 

To  accommodate  large  terrain  height  variations  "soft"  spring 
constants  were  chosen.  This,  however,  causes  the  vehicle  body  to  pitch 
and  roll  excessively.  To  avoid  this  problem,  an  attitude  control  scheme 
was  developed  [13].  From  small  angle  approximation,  the  body  pitch  and 
roll  errors  are  converted  to  vertical  foot  displacement  In  a  linear 
equation: 

AZi  -  -Xi  ya  +  Yi  «A  (2.15) 

where  X-j  and  Yf  are  the  x  and  y  components  of  the  foot  position  of  leg  1 
with  respect  to  the  center  of  gravity  of  the  body;  ya  and  $A  are 
the  body  pitch  and  roll  angles  measured  by  the  attitude  sensors. 


A 

During  support  phase  the  desired  velocity  for  attitude  control,  zq, 
can  be  computed  as 

ZD  *  kA  (  xi  ya  -  Yi  6a  )  (2.16) 

A 

where  kA  is  the  desired  attitude-control  gain,  and  zq  is  obtained  by 

.A 

integration  of  zq.  Then  the  desired  vertical  foot  position  and  velocity 
are  given  by 

ZD  *  zq  +  zd  (2.17) 

and 

zq  *  zj)  +  zd,  (2.18) 

P  .P 

respectively,  where  zd  and  zd  are  the  desired  vertical  foot  position 
and  velocity  from  the  motion  planning  block.  The  attitude  control 
system  associated  with  the  Jacobian  control  structure  In  Figure  2.4  is 
given  in  Figure  2.5  [11]. 

• 

2.5.2  State  Feedback  Control 

In  State  Feedback  Control,  the  actual  state  of  each  of  the  joints 
(position  and  velocity)  is  compared  with  the  desired  state  and  Is  used 
to  provide  control  Inputs  to  each  of  the  actuators,  usually  through  a 
linear  feedback  control  law.  Since  the  steady-state  error  and  the 
the  feedback  gains  are  Interrelated  for  the  overall  system  stability,  a 
feedforward  approach  is  employed  to  eliminate  steady-state  error 
without  increasing  the  gains  too  much.  For  this  purpose,  the  Inverse 
dynamic  model  of  the  leg  system  Is  used,  called  Inverse  plant. 

The  Inverse  plant  and  state  feedback  control  scheme  [33]  was  applied 
to  control  the  Monopod  system  [34].  The  Monopod  jystem  was  built  at  the 
Ohio  State  University  to  Investigate  the  energy  efficient  leg  of  the 


walking  machine  In  the  laboratory  and  consists  of  a  three-degree- of- 
freedom  leg  with  three-wheeled  cart.  The  basic  equation  describing  the 
state  feedback  control  law  Is 

V  =  K(X0  -  x)  +  Vp  (2.16) 

where  IMs  actual  actuator  voltage,  Vq  is  the  nominal  voltage  of  the 
actuator,  x.  Is  the  state  variables  (joint  angles  and  rates),  xj)  is  the 
desired  set  points  for  the  state  variables  and  K  Is  the  feedback  gain 
matrix.  The  overall  inverse  plant  and  state  feedback  control  structure 
Is  shown  fn  Figure  2.6  [33]. 

2.6  Summary 

Thl'.  chapter  has  presented  brief  review  of  existing  theory  and 
control  algorithms  of  legged  vehicle  locomotion  related  to  this 
dissertation.  The  results  of  the  mathematical  theory  that  has  been 
developed  for  gait  selection  and  implementation  are  employed  in  this 
work. 

The  supervisory  control  scheme  provides  a  basic  structure  In 
software  implementation  of  control  algorithms.  This  dissertation 
mainly  deals  with  the  motion  planning  block  of  the  supervisory  control 
scheme,  since  the  low-level  servo  control  Is  not  necessary  by  the 
graphic  simulation  of  the  vehicle.  However,  the  Jacobian  control 
described  in  section  2.5  was  Implemented  in  the  motion  execution  block 
for  control  of  the  OSU  Hexapod  and  will,  also,  be  implemented  for 
control  of  the  ASV.  Based  on  the  theory  presented  In  this  chapter,  the 
control  algorithms  for  locomotion  of  a  six-legged  vehicle  over  uneven 
terrain  are  discussed  In  the  following  chapters. 
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Chapter  3 

TERRAIN  AND  VEHICLE  SIMULATION 


3.1  Introduction 

The  principal  advantage  of  legged  vehicles  over  tracked  or  wheeled 
vehicles  may  be  superior  adaptability  to  rough  terrain  In  off-the-road 
applications.  The  major  thrust  of  this  dissertation  Is  to  Investigate 
the  locomotion  of  legged  vehicles  in  such  conditions  through  computer 
simulation.  As  an  Important-basis  for  this  work,  it  is  necessary  to 
define  a  proper  vehicle/terrain  model.  In  this  chapter,  the  vehicle  and 
terrain  models  are  given. 

Terrain  models  have  often  been  developed  for  two-dimensional  (2-D) 
analysis  [9].  When  three-dimensional  (3-D)  terrain  models  were  used, 
the  terrain  data  base  has  not  generally,  been  provided  Interactively 
[35].  However,  In  this  dissertation  work,  various  3-D  terrain  models 
may  be  generated  interactively  so  that  locomotion  over  different  kinds 
of  terrain  may  be  rather  simply  studied.  In  Section  3.2,  a  simple  but 
realistic  3-D  terrain  model,  called  a  prismatic  model  [35],  Is 
Introduced.  First,  the  terrain  profile,  which  Is  composed  of  a 
piecewise  linear  curve.  Is  Interactively  generated  using  a  three-axis 
joystick.  Then  the  desired. perspective  view  of  the  prismatic  terrain 
model  is  displayed  on  the  screen  by  changing  the  viewing  angle  with  the 
joystick. 
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The  vehicle  model  used  in  this  work  is  for  the  ASV  (Adaptive 
Suspension  Vehicle)  [20]  .  The  vehicle  is  a  hexapod  with  three  degrees 
of  freedom  in  each  of  its  six  legs.  The  kinematic  equations  relating 
foot  positions  and  rates  to  joint  angles  and  rates  are  relatively  simple 
since  the  leg  is  based  on  a  pantograph  mechanism.  These  equations  are 
derived  and  given  in  Section  3.3. 

Also,  due  to  the  mechanical  limits  of  the  joints,  the  reachable 
space  of  the  foot  is  confined  in  a  3-D  volume,  called  the  working 
volume.  The  working  volume  is  especially  important  in  this  work  since 
algorithms  for  leg  placement  and  liftoff  as  well  as  those  for  foothold 
selection  have,  been  based  upon  it.  The  general  equations  for  the 
working  volume  are  the  same  for  each  leg  and  are  also  developed  in 
Section  3.3.  The  locomotion  algorithms  that  use  the  working  volume  con¬ 
cept  are  presented  in  Chapter  4. 

The  ASV  is  a  very  complex  system  consisting  of  a  total  of  18 
degrees  of  freedom  and  more  than  13  mechanical  links  and  further  it 
will  encounter  a  myriad  of  terrain  conditions.  Because  of  this,  it  is 
necessary  to  provide  the  control  designer  with  convenient  tools  so 
that  he  may  interactively  experiment  with  various  parameters  of  the 
control  algorithms  and  terrain.  This  approach  is  just  that  of 
Computer-Aided  Design  (CAD)  and  has  been  developed  to  some  extent  in 
this  work  and  is  presented  in  Section  3.4.  As  stated  before,  the 
prismatic  terrain  model  may  be  interactively  generated.  Also, 
locomotion  of  the  ASV  over  the  terrain  may  be  viewed  in  any 
perspective.  In  addition  to  this,  the  Instantaneous  stability  margin 
is  displayed  as  well  as  the  position  of  each  of  the  feet  within  their 
constrained  working  volumes.  Finally,  this  makes  It  possible  to  alter 
such  parameters  as  the  leg  duty  factor. 
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3.2  Terrain  Model 

The  general  type  of  terrain  simulated  may  be  defined  as  a  3-0 
surface  in  which  the  heights  of  the  surface  points  are  not  constant  so 
that  slopes,  ditches,  holes,  obstacles,  etc.,  may  be  configured.  In 
order  to  reduce  the  complexity  of  the  simulation  while  maintaining  a 
reasonable  amount  of  generality,  a  terrain  model  is  developed  in  such 
a  way  that  the  cross  section  of  the  terrain  surface  along  one  of  the 
horizontal  axes  is  constant.  Furthermore,  the  cross  section  along  this 
axis  consists  of  a  piecewise  linear  curve.  This  model  may  be  termed  a 
prismatic  terrain  model  [35]. 

Since  the  body  of  the  vehicle  moves  on  the  terrain  with  a  full  slx- 
degree-of- freedom,  the  terrain  conditions  Imposed  on  the  vehicle  at 
some  instant  during  locomotion  should  be  somewhat  arbitrary  with  respect 
to  the  vehicle  posture.  The  prismatic  terrain  model  may  be  Inter¬ 
actively  defined  to  Impose  many  different  terrain  conditions  on  the 
vehicle  and  this  may  provide  a  rather  realistic  walking  environment. 

The  terrain  model  Is  defined  In  the  earth- fixed  coordinate  system 
(xE.yEtZE)  where  the  ze  axis  is  In  the  opposite  direction  of 
gravitational  acceleration  (positive  upward),  and  the  X£  and  y£  axes  are 
appropriately  defined  such  that  X£,  y£»  and  ze  are  mutually  orthogonal 
unit  vectors.  Once  the  earth  coordinate  system  is  fixed,  then  the 
terrain  model  Is  set  up  In  such  a  way  that  the  height  (ZE-component)  of 
the  terrain  changes  with  changing  distance  along  the  X£  axis.  The 
terrain  width  along  the  y£  direction  Is  fixed  to  some  constant  value. 

In  terrain  generation  ,  a  discrete  set  of  points  are  specified  in 
the  Ze-Xe  plane.  Then  the  terrain  profile  Is  generated  by  connecting 
two  adjacent  points  with  a  straight  line.  In  this  manner,  the  profile 


of  a  smooth  or  undulating  terrain  may  be  modelled  through  the  use  of  a 
piecewise  linear  curve.  Finally,  a  3-D  prismatic  terrain  model  maty  be 
obtained  by  Including  the  Y£-component  to  give  a  constant  cross  section 
In  this  direction.  Every  two  adjacent  points  of  the  profile,  along  with 
the  Y£-component,  compose  a  rectangular  plane  which  is  represented  by  a 
plane  equation  In  3-D  space.  After  generating  the  terrain  model,  the 
terrain  Information  Is  stored  as  an  array  of  records  In  the  computer 
main  memory.  Each  record  contains  the  coefficients  of  each  rectangular 
plane  equation  and  the  X£-components  of  two  adjacent  points  which  define 
the  left  and  right  boundaries  of  the  plane  In  the  X£  direction. 

Figure  3.1a  shows  a  typical  terrain  profile  which  is  Interactively 
generated  on  the  screen  of  the  HP  1350A  vector  graphics  display  using  a 
three-axis  joystick.  To  define  the  next  discrete  terrain  surface 
point,  the  cursor  (A)  Is  positioned  through  two  axes  of  the  joystick  to 
the  desired  position.  A  twist  of  the  joystick  sets  that  as  the  next 
point  and  connects  a  straight  line  to  It.  The  number  of  points  for  the 
terrain  profile  Is  limited  to  50  In  this  simulation. 

Figure  3.1b  gives  a  perspective  view  of  the  terrain.  After  the 
profile  has  been  defined,  then  two  axes  of  the  joystick  may  be  used  to 
define  the  viewing  angle  of  the  terrain.  Straight  lines  are  also  added 
In  the  y£  direction,  spaced  one  foot  apart  aTong  the  X£  axis.  These 
lines  enhance  the  visual  Image  of  the  terrain  by  Indicating  the  slope 
of  the  surface.  For  example,  the  surface  lines  for  a  steep  slope 
appear  to  be  more  densely  drawn  In  the  perspective  view  than  those  for 
a  level  surface. 
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3.3  Vehicle  Model 

The  hexapod  vehicle  model  used  in  most  part  in  this  work  is  for 
the  ASV  which  is  presently  under  construction  at  The  Ohio  State 
University.  The  ASV  is  to  be  the  first  fully  self-contained, 
computer-controlled  terrain-adaptive  walking  vehicle.  It  is  supported 
by  six  three-degree-of-freedom  legs,  each  with  a  special  kinematic 
configuration  (pantograph)  providing  a  simple  and  energy-efficient 
structure. 

3.3.1  Body  Model 

The  body  is  modelled  as  a  hexahedral  box  with  the  top  plane  wider 
than  the  bottom  plane,  which  allows  the  abduction/ adduction  motion  of 
the  legs.  The  six  legs  are  attached  to  the  body  at  the  outside  edges  of 
the  top  plane. 

Figure  3.2  shows  a  perspective  of  the  simulation  model  of  the  ASV 
and  its  related  coordinate  systems.  The  earth- fixed  coordinate  system 
(xg,  VE>  *e)  bas  been  described  in  the  previous  section;  It  is  used  when 
specifying  the  absolute  position  and  velocity  of  the  body. 

In  order  to  describe  the  motion  of  the  vehicle  walking  over 
terrain,  it  Is  also  useful  to  define  a  body- fixed  coordinate  system 
which  is  a  reference  coordinate  system  for  the  operator's  commmands 
and  for  generating  foot  positions  and  velocities.  The  body  coordinate 
system  (xg,  yg,  zg)  is  fixed  at  the  top  plane  of  the  body  and  its  origin 
Is  located  at  the  center  of  the  plane.  The  xg  axis  Is  along  the  body 
longitudinal  axis  and  directed  forward  while  the  zg  axis  Is  In  the 
direction  of  the  normal  vector  to  the  body  plane  (positive  upward). 


The  yg  axis  is  defined  by  the  vector  cross-product 
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The  transformation  from  the  body  coordinate  system  to  the  earth- 
fixed  coordinate  system  has  been  defined  through  the  use  of  a  4X4 
homogeneous  transformation  matrix,  H,  such  that  [36] 
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where  [Xf  Yf  Zg]T  and  [Xg  Yg  Zg]T  are  position  vectors  to  a  common  point 
as  referenced  to  the  appropriate  coordinate  systems.  The  matrix  H  may 
be  partitioned  into  submatrices: 
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(3.3) 


where  Hr  Is  a  3X3  orientation  matrix  and  is  a  3X1  position  vector. 
Any  velocity  vector  defined  in  the  body  coordinate  system  (B>0  may  be 
transformed  to  the  earth- fixed  coordinate  system  (^v)  by  premultiply¬ 
ing  by  the  orientation  matrix  Hr: 

*  Hr  (3.4) 

The  vector  ^  represents  the  position  of  the  center  of  the  bo<1y  In 
the  earth-fixed  coordinate  system  and  the  orientation  matrix  Hr 
expresses  the  rotational  property  between  the  earth  and  body  coordinate 


systems.  Thus,  the  homogeneous  transformation  matrix  H  can  be  easily 
obtained  by  the  relative  transformation  method  [36]  and  continuously 
updated  according  to  the  movement  of  the  vehicle.  This  will  be 
discussed  in  detail  in  a  later  chapter. 

3.3.2  Leg  Model 

The  six  legs  of  the  ASV  have  been  designed  by  employing  a  special 
kinematic  configuration,  called  a  pantograph.  Each  leg  has  two 
one-degree-of-freedom  sliding  joints  which  are  located  on  a  base 
plate.  One  sliding  joint  provides  up/down  motion  and  the  other  gives 
forward/backward  motion  of  the  foot  with  respect  to  the  base  plate. 

Due  to  the  kinematic  characteristics  of  the  pantograph,  the  motion  of 
the  foot  tip  appears  to  be  proportional  to  the  motion  of  each  sliding 
joint  through  a  fixed  ratio.  Each  leg  is  placed  on  a  base  plate  which 
Is  attached  to  the  body  through  hinges  to  allow  abduction/adduction 
motion  of  the  leg.  Thus,  the  abduction/ adduction  motion  at  the  hip 
gives  a  third  kinematic  degree-of-freedom  for  each  leg.  The  dimensions 
of  the  vehicle  are  shown  In  Figure  3.3  along  with  the  leg  model  for 
the  simulation. 

3.3.2. 1  Kinematics  of  the  Leg 

Because  of  the  relatively  simple  geometry  of  the  pantograph  leg, -the 
kinematic  equations  may  be  easily  obtained  by  using  simple  geometrical 
principles.  Let  the  foot  position  of  leg  1  be  (Xp,Yp,Zp)  in  the  body 
coordinate  system.  As  shown  In  Figure  3.4,  the  foot  position  of  leg  1 
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(c)  Front  View 

Figure  3.3.  Geometry  of  the  ASV, 
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Figure  3.4.  Geometrical  Computation  of  the  Foot  Position. 
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(front  left  leg)  is  given  by 


Xf 

5d2  +  hx 

Yf 

s 

(5t 3  -  4di )  sine  +  *4  cose  +  hy 

Zf 

-  (5*3  -  4di )  cose  +  *4  sine 

where  e,  dj.,  and  d2  are  the  joint  variables.  Since  all  of  the  six  legs 
have  the  same  basic  configuration,  the  kinematic  equations  for  each  may 
be  obtained  from  Eq.  (3.5)  with  appropriate  sign  changes. 

The  knee  position  (Xk,Yk,Z|<)  of  leg  1  may  be  obtained  by  also 
applying  simple  geometrical  principles  and  is  given: 
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-(*2  s1n«  +  dl)  cos®  +  *4  sine 
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Again,  since  all  of  the  six  legs  have  the  same  basic  configuration,  the 
equations  for  the  positions  of  the  knees  of  the  other  legs  may  be 
obtained  from  Eq.  (3.6)  with  appropriate  sign  changes. 

The  Inverse  kinematic  equations  may  be  derived  from  Eq.  (3.5)  In  the 
following  manner.  From  the  equation  of  the  first  row,  d2  is 


Squaring  the  both  sides  of  the  equations  of  the  second  and  third  rows, 
respectively,  yields 


a2sin2e  +  2at4Sinecose  +  442cos2e  3  (Yp  -  hy)2 


a2cos2e  -  2ai4Sinecose  +  44Zsinze  3  Zp2, 


2 


(3.10 


where  a  3  543  -  4d}.  Adding  Eqs.  (3.9)  and  (3.10)  and  then  rearranging 
the  result,  it  is  obtained 


di  3  j  (Si3  -  /(Yp  -  hy)z  +  ZFz  -  44*). 


(3.11 


From  the  equation  of  the  third  row  of  Eq.  (3.5),  e  can  be  solved  by  the 
following  trigonometric  substitutions 
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Then  e  is  given  by 


e  3  4  -  tan~l 


(3.17 


For  the  computer  simulation  of  the  ASV,  only  the  Inverse  kinematic 
equations  are  used.  For  velocity  control  of  the  vehicle,  however,  the 
Inverse  of  the  Jacobian,  which  converts  foot  velocity  commands  to 
joint  rate  commands  for  the  leg  actuators,  is  required.  Hence,  the  3X3 


Jacobian  matrix,  J,  is  defined  as  follows: 
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Yp  *  J  dx 
Zp  d2 


(3.18) 


where 
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The  elements  of  J  for  leg  1  may  be  obtained  by  partial  differentiation 
of  the  right  hand  side  of  Eq.  (3.5),  which  gives 


J  3  a  cose  -  £4  sine  -4  sine  0 

a  sine  +  £4  cose  4  cose  0 


(3.20) 


where  a  3  5*3-4di.  Since  the  Jacobian  matrix  is  so  simple,  its  inverse 
may  be  easily  obtained.  The  inverse  Jacobian,  J"1*  is  given  by 
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3. 3. 2. 2  Working  Volume  for  a  Leg 

The  mechanical  limits  of  the  joints  restrict  the  motion  of  the  leg 
and  this  Isa  major  factor  to  consider  when  developing  control 
algorithms  for  the  legged  vehicle.  Therefore,  it  is  necessary  to  define 
the  volume  of  points  in  space  that  the  foot  of  a  leg  can  reach,  called 
the  working  volume,  in  order  to  appropriately  specify  the  foothold 
positions  that  insure  that  all  the  joint  positions  are  within  their 
mechanical  limits. 

Since  each  leg  has  the  same  geometrical  configuration  and  joint 
limits,  the  working  volumes  of  each  leg  are  identical.  The  limits  of 
the  joint  variables  for  leg  1  are  as  follows: 


-10°  <  e  <  21° 
o'*  <  dj_  <  r 
•0.6*  <  d2  <  0.6*. 


(3.22) 


Another  condition  which  may  be  considered  is  the  case  where  the  leg 
stretches  out  in  such  a  way  that  all  the  joints  of  the  pantograph  are 
located  on  a  straight  line.  This  may  be  expressed  in  the  following 
equation: 


(£3  -  di)2  +  d£2  *  (2*i)z. 


(3.23) 


However,  the  joint  limits  given  in  Eq.  (3.22)  guarantee  that  this  case 
will  never  occur. 

The  analytic  expressions  for  the  working  volume  m«y  be  derived  from 
Eq.  (3.5).  Without  loss  of  generality,  let  hx  a  hy  *  0.  Then,  Eq.  (3.5) 


reduces  to 


XF  =  5d2 

Yp  *  a  sine  +  £4  cose  (3.24 

Zp  =  -a  cose  +44  sine 

where  a  *  5*3  -4di»  By  eliminating  e  from  Eq.  (3.24),  the  following  is 
obtai ned: 

Yp2  +  ZF2  *  a2  +  A42.  (3.25 

From  Eqs.  (3.22)  and  (3.24),  the  limits  of  (a)  are  given  by 

543  -  4  <  a  <  5*3.  (3.26 

Also,  eliminating  (a)  from  Eq.  (3.24)  gives 

Zp  *  -Ypcote  +  £4/sine.  (3.27 

With  consideration  of  the  joint  limits  given  in  Eq.  (3.22)  and  (a)  in 
Eq.  (3.26),  the  Eqs.  (3.25)  and  (3.27)  define  the  working  volume  as  a 
sector  of  an  annulus  in  the  Yp-Zp  plane  using  the  leg  dimensions  given 
in  Figure  3.3.  The  3-D  working  volume  may  be  obtained  by  extending  the 
cross  section  In  the  Yp-Zp  plane  to  the  xp  direction  as  shown  In 
Figure  3.5.  Note  that  the  origin  of  the  coordinate  system  (xp.yp.zp) 
is  located  at  the  hip  socket  and  each  axis  Is  parallel  to  the 
correspondi ng  axis  of  the  body  coordinate  system. 

3.4  Computer-Aided  Design 

Computer-Afded  Design  (CAD)  systems,  are  now  widely  used  in  many 
engineering  fields,  which  has  largely  resulted  from  the  development  of 
sophisticated  3-D  interatlve  graphics  hardware  and  software.  The 
purpose  of  CAD  systems  is  to  reduce  costly  prototyping  by  substituting 
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Figure  3.5.  Dimensions  and  Shape  of  the  Working  Volume  for 
*  the  Legs  of  the  ASV-84  Vehicle. 


3-D  computer  models  for  physical  models.  The  computer  simulation  in 
this  work  has  been  accomplished  using  the  PDP-11/70  minicomputer  along 
with  the  HP  1350A  vector  graphics  device  which  displays  the  perspective 
view  of  the  vehicle  and  terrain,  the  top  view  of  the  vehicle,  and 
finally  its  support  pattern.  The  overall  display  is  shown  in  Figure  3.6 
and  represents  a  CAD  approach  to  the  development  of  the  controls  for  the 
ASV. 

The  perspective  view  of  the  vehicle  and  terrain  allows  one  to 
examine  the  overall  performance  of  the  control  algorithms  for  3-D 
locomotion  of  the  ASV.  As  has  been  discussed  in  Section  3.2,  there  is 
considerable  flexibility  in  designing  the  terrain  cross-section  and  in 
specifying  the  viewing  angle. 

The  top  view  is  generated  by  projecting  the  vehicle  onto  the  Xg-Yg 
plane  of  the  body  coordinate  system.  The  top  view  of  the  constrained 
working  volumes  for  each  of  the  legs  is  also  Included.  The  constrained 
working  volume  for  a  leg  Is  just  the  rectangular  solid  volume.  Inside 
the  actual  working  volume,  in  which  the  leg  control  algorithms  attempt 
to  maintain  the  foot  motion  (see  Chapter  4).  In  addition  to  the 
constrained  working  volume,  the  position  of  the  foot  within  it  is 
clearly  shown.  Furthermore,  the  support  foot  which  Is  "nearest”  the 
edge  of  Its  constrained  working  volume  (according  to  the  algorithms 
developed  in  Chapter  4),  Is  Indicated  by  a  line  from  the  foot  to  the 
critical  edge  (see  Chapter  4).  Finally,  any  problems  with  leg 
collisions  may  be  determined  by  taking  note  of  the  top  view. 

The  support  pattern  [23]  Is  displayed  in  order  to  investigate  the 
degree  of  static  stability  during  the  motion  of  the  vehicle.  The  sup¬ 
port  pattern  Is  a  polygon  projected  onto  the  Xe-Ye  plane  of  the  earth- 
fixed  coordinate  system,  and  the  projection  of  the  center  of  gravity  is 
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also  shown.  A  line  from  the  center  of  gravity  to  the  nearest  edge  of 
the  support  polygon  gives  an  Indication  of  the  Instantaneous  stability 
margin,  and  this  Is  also  computed  and  displayed. 

The  stability  margin  Is  computed  in  the  following  manner.  Let  two 
adjacent  vertices  of  the  support  polygon  be  (X-j.Y-j)  and  (Xi+i.Yf+i)  in 
the  earth- fixed  coordinate  system.  Then  the  equation  of  the  line  which 
passes  through  two  vertices  may  be  expressed  as 


AX  +  BY  +  C  *  0 


where 


A  =  Yi+1  -  Yi 
B  =  Xi  -  Xi+i 
C  =  Y-jX-j+i  -  X-f Y-f+i . 


(3.28) 


(3.29) 

(3.30) 

(3.31) 


Assume  that  the  center  of  gravity  of  the  vehicle,  (Xc,Yc),  is  given. 
The  perpendi cul ar  distance  from  the  center  of  gravity  to  the  line,  D-j, 
is  obtained  as  follows: 


IAXC  +  BYC  +  Cl 


W  +  B2 


(3.32) 


Hence,  the  minimum  of  the  Dj’s  gives  the  stability  margin,  and  the 
corresponding  edge  of  the  support  pattern  is  specified. 

The  center  of  gravity  of  the  vehicle  is  constantly  changing,  but  may 
be  computed  since  the  mass  of  the  body  and  the  legs  as  well  as  their 
positions  are  given.  However,  for  simplicity,  the  center  of  gravity  . 
used  in  this  work  for  the  ASV  Is  assumed  to  be  the  geometric  center  of 
the  body. 


The  computation  of  the  stability  margin  as  just  described,  where 
It  Is  measured  with  respect  to  the  vertical  projection  of  the  center 
of  gravity,  does  not  Include  the  dynamic  effects  of  the  Inertial 
acceleration  of  the  body  and  legs.  As  such,  even  though  the  vertical 
projection  of  the  center  of  gravity  Is  contained  within  the  support 
pattern,  the  vehicle- may  be  unstable  since  the  projection  of  the  center 
of  gravity  along  the  total  acceleration  vector  (both  gravitational  and 
Inertial  components)  may  fall  outside  the  support  pattern.  In  this 
regard  In  the  general  case,  the  stability  margin  should  be  measured 
while  Including  the  inertial  effects  of  the  massive  legs  and  the  body. 

The  inertial  and  gravitational  forces  on  the  body  and  legs  are 
balanced  by  the  external  forces  acting  on  the  vehicle  at  Its  feet.  The 
net  effect  of  these  ground  reaction  forces  may  be  replaced  by  a  single 
force  (which  Is  the  sum  of  all  ground  reaction  forces)  acting  at  the 
zero  moment  point  [3]  or  center  of  pressure.  The  zero  moment  point  Is 
defined  as  the  point  In  the  support  plane  about  which  the  moment  due  to 
the  vertical  ground  reaction  forces  Is  equal  to  zero  [3].  The 
requirement  for  stability- Is  that  the  zero  moment  point  lies  within  the 
support  pattern  [23].  When  the  Inertial  accelerat-ons  are  zero,  then 
the  zero  moment  point  Is  just  the  vertical  projection  of  the  center  of 
gravity  onto  the  support  plane. 

Since  the  complete  specifications  for  the  mass  of  the  body  and  legs 
of  the  ASV  have  not  yet  been  made,  the  stability  margin  for  the  ASV  has 
not  been  based  on  the  zero  moment  point  concept.  However,  In  previous 
simulation  work  for  the  OSU  Hexapod  walking  over  flat  level  terrain,  the 
zero  moment  point  was  computed  In  the  following  w*y. 

Assume  that  all  the  leg  mass  Is  concentrated  at  the  knee  joint.  (A 
large  motor  at  the  knee  Is  the  most  of  the  leg  mass.)  Also,  assume 
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that  the  rotational  accelerations  about  the  pitch  and  roll  axes  are 
negligible  since  the  motion  Is  for  flat  level  terrain.  Then  the 


following  equations,  with  components  expressed  in  the  body  coordinate 
system,  are  satisfied: 


IMx  *  YZMFz  -  ZZMFy 
6 

=iI0[Yimi(-9  +  Zl) 


(3.33) 


-  ZjmfYj]  , 


I  My  =  "xZMFz  +  zZMFx 

=1IoC“Xin,l(“9  +  Zl)  +  • 


(3.34) 


where  ][MX  and  £My  are  total  moments  about  the  xg  and  yg  axes, 
respectively;  XZM,  YZM,  and  ZZm  are  the  components  of  the  position 
vector  for  the  zero  moment  point;  Xj,  Yj,  and  Zj  are  the  components  of 
the  position  vector  for  the  center  of  mass  of  the  body  (1*0)  or  leg  1; 

X-j ,  Y-j ,  and  Zj  are  the  translational  accelerations  of  the  center  of  mass 
1;  m-f  Is  mass  of  the  body  (1*0)  or  leg  1;  g  Is  the  gravitational 
acceleration;  Fx,  Fy,  and  Fz  are  external  ground  reaction  forces  onto 
the  feet.  The  external  forces  are  given  by 


Fx  3  I  mixi 
1*0 


(3.35) 


Fy  *  l  miYf 
y  1*0 


(3.36) 


Fz  “  I»"l(-9  +  zl)- 
1*0 


(3.37) 
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Note  that  the  gravitational  vector  is  assumed  to  be  in  the  -zg  direction 
in  Eq.  (3.37);  this  Is  the  case  when  the  body  is  parallel  to  a  level 
terrain.  Also,  since  the  vehicle  is  always  contacting  the  ground,  the 
zero  moment  point  is  always  located  on  the  ground  plane.  Thus,  Zzm  is 
the  body  height  which  is  known.  From  Eqs.  (3.33)  and  (3.34),  Xzm  and 
Yzm  may  be  obtained: 


xZm  * 


-iHy  +  Zzmfx 


(3.38) 


yZM  = 


I"x  +  zZMpy 


(3.39) 


The  translational  acceleration  of  the  body  and  the  angular 
accelerations  of  each  joint  are  calculated  by  first  order  numerical 
differentiation  of  the  body  velocity  and  the  joint  rates.  Using  those 
values,  the  translational  acceleration  at  the  center  of  mass  of  the  leg 
Is  computed  through  use  of  the  kinematic  equations  given  In  [37]. 

Figure  3.7  shows  the  perspective  view,  the  top  view,  and  the 
support  pattern  of  the  OSU  Hexapod  during  locomotion  over  flat  level 
terrain.  It  Is  noted  In  this  figure  that  the  zero  moment  point  is  not 
coincident  with  the  vertical  projection  of  the  center  of  gravity  when 
the  OSU  Hexapod  suddenly  halts. 

By  displaying  three  different  features  of  the  motion  of  either  the 
ASV  or  the  OSU  Hexapod  vehicle  on  the  graphics  display,  evaluation  of 
the  control  algorithms  developed  Is  enhanced.  Also,  it  Is  possible  to 
interactively  change  various  parameters  for  the  vehicle  and  terrain  and 
to  Immediately  view  the  effect  on  the  control.  The  Initial  effective¬ 
ness  of  Interactively  using  the  graphics  display  will  hopefully  motivate 
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the  extended  use  of  Computer-Aided  Design  In  the  development  of  the 
control  algorithms  for  legged  vehicles. 

3.5  Summary 

In  this  chapter,  terrain  and  vehicle  models  were  presented. 
Furthermore,  the  Computer-Aided  Design  concept  was  Introduced  for 
development  of  the  control  algorithms  using  the  Interactive  graphics 
device. 

The  terrain  model  developed  Is  simple  but  realistic  enough  to 
Investigate  the  legged  vehicle  locomotion  problem  over  rough  terrain. 
Based  on  the  2-D  terrain  profile,  the  3-D  prismatic  terrain  model  may 
Include  various  kinds  of  terrain  Irregularities  such  as  holes,  ditches 
or  slopes.  Furthermore,  the  terrain  can  be  Interactively  generated  on 
the  graphics  display  using  a  three-axis  joystick. 

The  vehicle  model  of  the  ASV  has  been  defined  and  its  kinematic 
equations  have  been  derived.  From  the  mechanical  joint  limits,  the 
working  volume  of  the  leg  has  been  computed  in  3-D  space,  which  will  be 
an  important  factor  In  developing  control  algorithms  for  locomotion  over 
uneven  terrain  as  presented  In  the  next  chapters. 

Finally,  by  displaying  three  different  features  of  the  motion  of  the 
vehicle  on  a  graphics  display:  the  perspective  view  of  the  vehicle  on 
the  terrain,  the  top  view,  and  the  support  pattern,  control  algorithms 
developed  may  be  more  simply  evaluated  by  observation.  The  ease  of 
evaluation  along  with  Interactive  change  of  various  parameters  of 
control  algorithms  may  motivate  the  extended  use  of  Computer-Aided 
Design  In  the  development  of  the  control  algorithms  for  legged  vehicle 
locomotion. 


Chapter  4 

OMNIDIRECTIONAL  CONTROL 

4.1  Introduction 

As  discussed  in  Chapter  2,  the  operation  of  the  OSU  Hexapod,  using  a 
three-axis  joystick  for  supervisory  control,  was  divided  Into  three 
different  control  inodes— Cruise  mode,  Turn-In-place  mode  and  Side¬ 
stepping  mode.  This  approach  was  taken  in  order  to  avoid  the  complexity 
of  leg  trajectory  planning  for  unrestricted  three-axis  joystick  control 
[8].  The  complexity  mainly  stems  from  the  difficulty  of  changing  the 
locomotion  period  from  mode  to  mode  without  stopping  the  vehicle.  How¬ 
ever,  experience  with  the  previous  control  algorithm  now  makes  It 
possible  to  develop  a  new  one  which  Integrates  the  above  three  joystick 
control  modes  Into  one  so  that  omnidirectional  control  is  provided,  and 
this  Is  the  subject  of  the  present  chapter. 

Previously,  In  [8],  the  period  was  defined  separately  for  each 
mode  as  the  ratio  of  a  particular  stride  length,  associated  with  the 
basic  motion  In  that  mode,  and  a  component  of  body  translational  or 
rotational  rate.  This  definition  of  the  period  resulted  in  the  need  to 
limit  the  motion  of  the  body  In  each  mode.  For  Instance,  the  turning 
radius  and  the  body  crab  angle  [8]  are  limited  In  the  Cruise  mode,  the 
lateral  and  longitudinal  velocity  components  are  forced  to  zero  In  the 
Turn-In-place  mode,  and  the  turning  rate  and  the  longitudinal  velocity 
component  are  forced  to  zero  In  the  Side-stepping  mode.  Experience 


with  the  algorithm  indicated  that  the  legs  would  attempt  to  go  out  of 
their  kinematic  limits  unless  the  motion  was  sufficiently  restricted. 

In  order  to  combine  the  three  different  modes,  the  limitations 
Imposed  on  the  motion  of  the  vehicle  have  been  eliminated  so  that 
omnidirectional  control  Is  provided.  The  resulting  control  scheme  has 
been  applied  to  the  ASV  and  provides  the  basis  for  the  Close  Maneuvering 
mode  of  operation  [14]. 

The  basic  approach  of  the  new  algorithm  is  a  "bottom-up"  control 
strategy  [25]  In  which  the  period  is  selected  on  the  basis  of  reach¬ 
ability  of  each  leg.  In  other  words,  the  period  is  defined  such  that  the 
foot  positions  of  each  leg  are  specified  inside  or,  at  most,  on  the 
boundary  of  the  working  volume.  However,  the  original  working  volume 
cannot  be  used  directly  for  this  purpose  because  it  contains  some  reach¬ 
able  points  which  may  cause  Instability  during  the  vehicle  motion.  To 
ensure  static  stability,  the  concept  of  a  constrained  working  volume  Is 
used  and  Introduced  in  Section  4.2. 

In  Section  4.3,  an  algorithm  for  computation  of  an  optimal  period 
based  on  the  constrained  working  volume  Is  discussed.  The  optimal 
period  for  each  locomotion  cycle  is  determined  based  on  how  close  each 
leg  is  to  Its  own  kinematic  limits  in  time  and  phase.  This  optimal 
period  ensures  that  at  least  one  of  the  six  legs  fully  utilizes  its 
constrained  working  volume,  while  the  other  legs  are  inside  their  own 
kinematic  limits.  In  this  sense,  the  period  obtained  Is  optimal. 

In  periodic  gaits  as  used  in  this  work,  the  locomotion  period 
determines  the  leg  swing  time.  Mechanically,  the  leg  liftoff,  transfer 
forward,  and  placement  takes  at  least  a  finite  amount  of  time,  called 
the  transfer  time  threshold.  If  the  optimal  period  is  too  short  to  swing 


the  leg,  it  is  necessary  to  slow  down  the  vehicle  to  achieve  an 
appropriate  leg  sequencing  determined  by  the  transfer  time  threshold.  In 
this  regard,  the  deceleration  plan  [29]  is  presented  in  Section  4.4. 

4.2  Constrained  Working  Volume 

As  discussed  in  Chapter  3,  the  working  volume  of  the  foot  is  given 
by  the  mechanical  limits  of  the  joints.  It  is  noted  in  the  simulation 
that  the  center  of  gravity  of  the  vehicle  goes  outside  the  support 
pattern,  which  indicates  static  instability,  at  some  instants  during 
locomotion,  even  if  the  support  points  of  the  feet  are  constrained  to 
be  Inside  their  working  volumes.  The  problem  is  due  in  part  to  the 
fact  that  the  initial  criterion  for  foot  positioning  used  was  not 
based  on  the  stability  of  the  vehicle,  but  simply  on  the  kinematic 
limits  of  each  leg.  In  order  to  correct  this,  consideration  was  given 
to  further  constraining  the  movement  of  the  feet  inside  their  working 
volumes  so  as  to  ensure  static  stability. 

The  minimum  number  of  support  legs  to  maintain  static  stability  of 
the  vehicle  is  three;  that  Is,  the  triangular  support  pattern  is  the 
minimal  set  of  support  points.  In  the  wave  gaits  used  in  this  work, 
the  middle  leg  of  one  side  Is  in  the  support  phase  along  with  the 
front  and  rear  legs  of  the  other  side  for  the  tripod  gait  (g  *  1/2).  If 
the  stability  of  the  vehicle  Is  ensured  for  the  tripod  gait  by 
appropriately  constraining  the  support  points  of  the  three  contacting 
feet  within  a  subset  of  the  working  volume,  then  stability  for  other 
wave  gaits  which  have  more  than  three  legs  in  support  (g  >  1/2)  is  also 
guaranteed  for  the  same  constraints  on  the  support  points.  Figure  4.1 
Illustrates  the  case  to  be  corrected  where  the  triangular  support 
pattern  of  the  tripod  gait  shows  instability  for  the  ASV. 


The  foregoing  discussion  leads  to  the  use  of  the  constrained  working 
volume  in  the  control  algorithms.  It  is  defined  as  a  subset  of  the 
original  working  volume  for  each  leg  that  ensures  static  stability  when 
there  is  a  tripod  or  more  of  support.  The  constrained  working  volume 
sets  soft  limits  for  each  leg  so  as  to  exclude  points  from  the  working 
volume  that  may  lead  to  instability. 

To  simplify  the  control  algorithms,  the  constrained  working  volume 
for  each  leg  is  specified  as  a  rectangular  solid  volume.  Its  size  and 
location  is  set  by  six  parameters:  [X^i  ,  Yq,,  Z^i 3  and  [dXf,  dyj,  dzf]. 
[Xci,  Ycj,  ZCi]  give  the  center  of  the  constrained  working  volume  for 
leg  i  in  body  coordinates  while  [dXj,  dyj,  dz{]  are  its  dimensions  along 
the  body  axes.  Figu.*e  4.2  shows  the  top  and  side  views  of  the  original 
working  volumes  and  constrained  working  volumes  for  each  leg  of  the  ASY. 
The  solid  lines  represent  the  original  working  volume  and  the  dashed 
lines  represent  the  soft  limits  imposed  by  the  constrained  working 
volumes.  Note  that  both  the  top  and  bottom  sides  of  the  original  working 
volume  consist  of  curved  surfaces.  Thus,  the  solid  lines  merely 
represent  the  maximum  size  of  the  volumes  In  2-D  space. 

Stability  of  the  vehicle  motion  may  be  examined  by  changing  the 
location  of  the  soft  limits  of  each  leg  imposed  by  the  constrained 
working  volume.  It  is  noted  from  the  simulation  results  that  the  further 
the  locations  of  the  soft  limits  are  away  from  the  center  of  gravity  of 
the  vehicle,  the  better  the  stability  margin  of  the  vehicle  that  is 
obtained  (see  Figure  4.2a). 

It  must  also  be  stated  that  the  previous  discussion  considers  loco¬ 
motion  over  flat  level  terrain  only.  Locomotion  over  uneven  and  sloping 
terrain  Is  considered  In  the  next  chapter  and  further  adjustments  to 
maintain  static  stability  are  made. 
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4.3  Optimal  Period 

In  the  previous  algorithm  [8],  the  locomotion  period  for  periodic 
gaits  was  determined  by  the  ratio  of  an  appropriate  stride  length  to  a 
particular  body  velocity  component.  Since  this  approach  does  not 
explicitly  consider  the  kinematic  limits  of  a  leg,  there  was  no  way  to 
ensure  that  the  foot  position  of  each  leg  during  locomotion  would  be 
within  Its  working  volume.  In  fact,  there  were  times  when  a  leg  of  the 
OSU  Hexapod  reached  its  limit. 

However,  if  the  body  motion  were  constrained  to  be.  In  most  part.  In 
one  direction,  then  the  stride  length  could  be  set  as  needed  for  that 
motion  so  as  to  keep  the  legs  within  limits.  This  led  to  three 
different  modes  of  operation  for  the  OSU  Hexapod  for  forward/backward 
movement  (Cruise),  lateral  movement  (Side-stepping),  and  for  turning 
(Turn- in-place).  The  purpose  of  the  present  work  Is  to  consider  the 
kinematic  limits  of  the  legs  more  directly  so  that  the  body  motion  need 
not  be  artificially  constrained.  The  result  Is  that  the  three  modes 
will  be  integrated  into  one  which  provides  the  basis  for  omnidirectional 
control  of  the  ASV. 

In  order  to  find  the  optimal  period  based  on  the  constrained 
working  volume,  the  following  definitions  are  made. 

Definition  9:  The  temporal  kinematic  margin  of  leg  1,  t^.  Is  a 
predicted  time- to- go  until  the  foot- tip  of  leg  1  In  the  support  phase 
reaches  the  surface  of  the  constrained  working  volume  with  the  present 
foot  velocity. 


By  the  above  definition. 


where  i s  the  present  foot  vel oci ty  of  leg  1  and  d  i s  the  di stance 
from  the  support  point  to  the  surface  of  the  constrained  working 
volume  in  the  direction  of  vj. 

Definition  10:  The  maximum  instantaneous  support  period  for  leg  i, 
is  a  predicted  maximum  time  for  leg  1  to  be  within  Its  constrained 
working  volume  while  in  the  support  phase. 

It  Is  noted  from  Eq.  (2.5)  that  the  period  Is  proportional  to  the 
ratio  of  incremental  time  change  to  Incremental  phase  change.  For  each 
leg  during  the  support  phase,  the  value  of  the  support  phase  variable, 
4^,  which  Is  defined  In  Eq.  (2.10),  changes  from  0  for  touchdown  to  1 
for  lift-off.  Then  the  maximum  Instantaneous  support  period  for  each 
leg  may  be  computed  through  the  use  of  the  temporal  kinematic  margin, 
t^,  and  the  support  phase  variable,  4^,  as  follows: 


A  leg  which  has  the  minimum  value  of  the  maximum  Instantaneous 
support  period  is  called  a  "critical  leg",  and  the  boundary  surface  of 
the  constrained  working  volume  which  the  critical  leg  will  reach  at  the 
end  of  the  support  phase  Is  called  a  "critical  edge".  To  keep  each  leg 
Inside  Its  constrained  working  volume,  the  optimal  support  period,  t^,. 
should  be  taken  from  the  critical  leg.  that  Is, 


Then  the  magnitude  of  the  optimal  period,  t,  Is  given  by 


As  discussed  In  Chapter  2,  the  sign  of  the  period,  t,  determines 
whether  a  forward  or  backward  wave  gait  Is  Implemented.  Both  gaits 
Implement  the  same  sequence  of  leg  liftings  and  placlngs,  however.  In 
opposite  order.  While  optimal  wave  gaits  are  the  only  ones  used  In  this 
work,  there  Is  still  the  possibility  of  using  a  forward  or  backward 
sequence. 

The  forward  sequence  (which  gives  a  forward  wave  action  of  the  legs 
as  referenced  to  the  front  of  the  vehicle)  has  been  shown  to  be  opti¬ 
mally  stable  for  straight-line  forward  locomotion  and  will  be  used  when 
the  longitudinal  component  of  velocity,  vx.  Is  positive  (forward  move¬ 
ment).  The  backward  sequence  (actually  forward  when  referenced  to  the 
rear  of  the  vehicle)  will  be  used  when  vx  Is  negative.  While  there  Is  no 
guarantee  that  either  of  these  sequences  Is  optimal  for  arbitrary  body 
motion,  use  of  these  should  generally  enhance  stability. 

There  Is  one  final  consideration  In  changing  from  one  sequence  to 
the  other.  The  gait  change  from  the  forward  wave  gait  to  the  backward 
wave  gait,  or  vice  versa,  should  occur  only  when  the  magnitude  of  the 
body  translational  velocity  Is  small.  Otherwise,  the  sudden  sign 
change  of  the  period  during  high  speed  locomotion  mqy  lead  to  a  leg 
going  out  of  Its  constrained  working  volume.  This  can  be  more  clearly 
understood  through  an  example.  If  a  leg  Is  about  to  liftoff,  the  sign 
change  of  the  period  causes  that  leg  to  appear  as  If  It  has  just  touched 
down.  This  means  that  the  time  that  this  leg  spends  on  the  ground 
Increases.  For  high  body  velocity,  the  leg  will  quickly  reach  the 


limits  of  its  constrained  working  volume.  The  problem  Is  due  to  the  fact 
that  the  leg  motion  depends  on  both  the  locomotion  period  and  the  body 
velocity,  while  the  body  motion  depends  only  on  the  body  velocity. 

The  above  considerations  lead  to  the  graphical  representation  of  the 
gait  change  In  the  vx-vy  velocity  plane  as  shown  In  Figure  4.3,  where  vy 
Is  the  lateral  velocity  component.  The  outside  of  the  circle  represents 
the  high  speed  region  where  a  gait  change  should  not  occur.  There  are 
two  regions  Inside  the  circle.  The  band  region  where  no  gait  change 
occurs  provides  hysteresis  In  order  to  avoid  quick  gait  changes  for 
nominally  zero  forward  velocity.  The  values  of  a  and  b  are  chosen 
experimentally  as  a=0.05  ft/sec  and  b*0.2  ft/sec  In  this  work. 

4.4  Deceleration  Plan 

The  speed  of  leg  return  in  the  transfer  phase  Is  limited  by  the 
capabilities  of  the  joint  servo  motors.  Hence,  there  exists  a  minimally 
required  transfer  time,  called  the  transfer  time  threshold,  for  the  leg 
to  move  from  lift-off  to  touchdown.  For  legged  vehicle  locomotion,  the 
commanded  transfer  time  of  each  leg  must  be  greater  than  the  transfer 
time  threshold. 

When  the  optimal  period  is  obtained  as  described  in  the  previous 
section,  it  Is  necessary  to  check  whether  It  satisfies  the  condition 
Imposed  by  the  transfer  time  threshold.  If  not,  the  vehicle  speed  must 
be  reduced. 

The  foot  velocity  In  the  support  phase  is  linearly  related  to  the 
body  velocity  components  (note  Eq.  (5. SI)  In  Chapter  5).  From  Eqs.  (4.1) 
through  (4.3), 


where  C-j  Is  a  proportionality  factor  for  leg  1  which  Is  constant  for  a 
specific  foot  position,  leg  phase,  and  duty  factor.  Also,  the  transfer 


time,  tr,  Is  related  to  t  such  as 


Itr  I  3  (1  -  e)  It  I  - 


(4.6) 


Let  the  transfer  time  threshold  be  xnt,.  .  If  t„  Is  less  than  t_  .  , 

Rth  R  Rth 

then  It  Is  necessary  to  slow  down  the  body  velocity  components  to 
limit  the  transfer  time  to  be  xRth.  Let  the  ratio,  y,  of  the  absolute 
values  of  tR  and  xRth  be 


'W  ' 


(♦.7) 


Using  the  relation  of  tr  and  T  given  In  Eq.  (4.6),  the  period  after 
slowdown,  t'.  Is  given  by 


....  'W 
lx  1 

From  Eqs.  (4.6)  through  (4.8),  the  following  Is  obtained: 


k'l  ItI  * 

Substituting  Eq.  (4.5)  Into  Eq.  (4.9)  yields 
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(4.10) 


Again,  from  the  linear  relationship  between  the  foot  velocity  In  the 
support  phase  and  the  body  velocity,  y  may  be  a  slowdown  factor  for  the 
body  velocity. 


I 


r » 


* 


8 


Thus,  whenever  the  transfer  time  of  the  optimal  period  Is  less  than 
the  transfer  time  threshold,  the  slowdown  factor  Is  computed  and  the  six 
body  velocity  components  are  reduced  accordingly.  The  value  of  the 
transfer  threshold  time  Is  chosen  to  be  0.5  sec  In  this  simulation 
work. 

4.5  Summary 

In  this  chapter,  the  algorithm  for  Integration  of  the  three 
different  joystick  control  modes,  which  were  previously  defined  for  the 
OSU  Hexapod,  has  been  described.  This  Integration  makes  It  possible  to 
Implement  the  Close  Maneuvering  Mode  of  operation  for  the  ASV  which 
provides  for  omnidirectional  control. 

The  Integration  Is  based  on  the  concept  of  the  constrained  working 
volume  which  provides  soft  limits  for  leg  motion  In  order  to  regulate 
leg  stepping  so  as  to  maintain  static  stability  for  the  vehicle. 

Since  the  dimensions  of  the  constrained  working  volume  may  be  easily 
changed  In  the  simulation.  It  Is  possible  to  examine  changes  In 
stability  for  different  dimensions  of  the  constrained  working  volume, 
and  this  Is  discussed  In  Chapter  5. 

The  optimal  period  was  defined  In  such  a  way  that  at  least  one  of 
the  six  legs  fully  utilizes  Its  constrained  working  volume,  while  the 
other  legs  are  maintained  inside  these  limits.  Thus,  the  optimal  period 
ensures  that  each  leg  moves  within  Its  constrained  working  volume  during 
locomotion  over  uneven  terrain.  Also,  in  this  chapter  a  simple 
algorithm  has  been  developed  to  choose  between  forward  and  backward  wave 
gaits,  and  hopefully  it  will  enhance  stability  for  arbitrary  body 


Finally,  a  deceleration  plan  has  been  presented  to  slow  down  the 
vehicle  speed  In  order  to  satisfy  the  minimum  transfer  time  capability 
of  the  legs.  The  slowdown  factor  was  defined  based  on  the  notion  that 
the  Inverse  of  the  optimal  period  Is  linearly  related  to  the  magnitude 
of  the  body  velocity.  Thus,  when  the  transfer  time  obtained  from  the 
optimal  period  Is  shorter  than  the  transfer  time  threshold,  each  of  six 
velocity  components  is  reduced  by  the  slowdown  factor. 

The  flow  chart  for  the  algorithm  developed  in  this  chapter  Is  given 
In  Figure  4.4.  The  algorithm  provides  an  essential  means  to  implement 
the  Close  Maneuvering  Mode  of  operation  for  the  ASY  which  provides  for 
omnidirectional  control  over  uneven  terrain.  Actual  control  algorithms 


for  locomotion  over  uneven  terrain  will  be  discussed  in  Chapter  5. 
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Figure  4.4.  Flow  Chart  for  the  Algorithm  to  Provide 
Omnidirectional  Control. 


Chapter  5 


In  this  chapter,  control  algorithms  for  the  Close  Maneuvering  Mode 
of  operation,  which  has  been  described  in  Section  2.3,  over  uneven 
terrain  are  presented  by  employing  the  theoretical  results  for  the 
gaits,  the  vehicle/ terrain  models,  and  the  algorithms  for  omnidirec¬ 
tional  control  developed  in  the  previous  chapters.  The  control 
algorithms  developed  in  the  previous  research  have  mainly  focused  on 
locomotion  over  flat  level  terrain  [83,  or  some  efforts  have  been  made 


In  developing  control  algorithms  for  uneven- terrain  locomotion 
[3,9,11,12].  Based  on  the  previous  work,  the  complete  control 
algorithms  for  locomotion  over  uneven  terrain  are  developed  and 
discussed  In  this  chapter. 

The  terrain  models  used  in  this  work  are  assumed  that  they  are  free 
of  holes  or  large  obstacles,  because  the  Close  Maneuvering  Mode  of 
operation  is  suitable  for  reasonably  rough  terrain.  These  terrain 
models  are  referred  to  as  "Type-0"  terrain  by  HI  rose  et  al.  [6].  They 


may  be  simulated  by  the  prismatic  terrain  models  described  In  Section 
3.2. 

The  control  algorithms  developed  are  based  on  the  supervisory 
control  scheme  described  In  Section  2.4.  According  to  the  basic 


$ 


hierarchical  structure  of  the  supervisory  control  scheme  as  shown  In 


Figure  2.3,  there  are  two  functional  blocks:  the  motion  planning  and 
execution  blocks.  Since  the  motion  execution  block  is  replaced  by  the 
vehicle/ terrain  simulation  block  in  this  work  (see  Chapter  6),  only  the 
motion  planning  algorithms  are  discussed  in  this  chapter. 

The  motion  planning  algorithms  control  the  desired  body  and  leg 
trajectories  with  feedback  of  contact  and  proximity  information  from  the 
leg  subsystems.  In  the  computer  simulation  of  this  work,  proximity 
sensors  are  simulated  in  software  which  are  assumed  to  measure  the 
distance  from  the  foot- tip  to  the  support  surface  in  the  direction  of 
the  gravity  vector.  The  software  simulation  of  a  proximity  sensor  is 
discussed  in  detail  in  Section  5.2.  Also,  contact  sensors  are 
simulated  in  the  same  way  as  proximity  sensors. 

Body  motion  planning  algorithms,  which  are  described  in  Section 
5.3,  generate  the  body  position  and  velocity  set-points  using  the 
operator's  velocity  commands  and  information  about  the  local  terrain 
surface.  The  local  terrain  surface  Is  estimated  by  using  six  points  of 
estimation  based  on  contact  and  proximity  information  from  the  six  legs 
through  linear  regression  [3].  Based  on  the  estimated  plane  of  support, 
algorithms  for  body  attitude  and  altitude  control  are  developed  in 
this  section. 

Leg  motion  planning  algorithms  are  designed  to  generate  the  foot 
trajectory  for  the  support  and  transfer  phases  and  to  choose  the  desired 
foothold  for  the  transfer  phase  based  on  the  body  velocity  and  the 
constrained  working  volumes  for  the  legs.  Furthermore,  proximity 
sensors  are  used  for  the  transfer  trajectory  in  order  to  avoid  foot 
contact  with  terrain  surface.  Discussions  about  leg  trajectory 
generation  are  made  in  Section  5.4. 
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An  algorithm  to  Increase  the  stability  of  the  vehicle  over  sloped 
terrain  by  dynamically  changing  the  positions  of  the  constrained  working 
volumes  for  the  middle  legs  according  to  the  slope  angle  Is  presented  in 
Section  5.5.  Since  the  size  of  the  constrained  working  volume  of  each 
leg  also  affects  the  stability  margin,  discussions  about  the  change  of 
the  size  of  the  constrained  working  volume  according  to  the  vehicle 
speed  are  made  in  this  section. 

5.2  Proximity  Sensors 

Recently,  the  use  of  sensing  technology  to  endow  a  robot  with  a 
greater  degree  of  “intelligence"  in  dealing  with  Its  environment  is 
receiving  increased  attention.  Moreover,  for  the  multi legged  robot 
vehicle,  the  Information  about  the  walking  environment  provided  by  the 
sensor  systems  is  essential  for  adaptive  locomotion  over  uneven  terrain. 

For  omnidirectional  movement  In  the  Close  Maneuvering  Mode  of 
operation  for  the  ASV,  full  terrain  preview  is  not  provided  since  the 
optical  radar  system  used  is  limited  to  scanning  in  the  front  of  the 
vehicle.  On  the  other  hand,  proximity  sensors  will  be  used  to  good 
advantage  to  sense  the  local  terrain  conditions  under  each  foot. 
Hopefully,  these  will  allow  each  leg  to  adapt  to  the  terrain.  In  the 
simulation  work  of  this  dissertation,  it  is  assumed  that  proximity 
sensors  are  available  and  these  will  be  integrated  into  the  control 
algorithms  In  order  to  facilitate  leg  placement  onto  the  terrain 
surface  and  to  avoid  foot  contact  with  terrain  surface  during  the 
transfer  phase. 

Broerman  recently  surveyed  the  proximity  sensing  techniques  and 
Implemented  a  proximity  sensor  system  based  on  ultrasonic  ranging  for 
the  OSU  Hexapod  [38].  The  proximity  sensors  were  attached  to  the  lower 
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limb  segment  of  each  leg  In  order  to  measure  the  foot  altitude  and  to 


approximately  control  the  position  and  velocity  of  each  leg  while  in  Its 


transfer  phase  of  motion,  especially  during  foot  descent. 


In  the  computer  simulation  of  this  work,  each  leg  of  the  ASV  Is 


simulated  in  software  to  be  equipped  with  a  proximity  sensor,  which  is 


assumed  to  measure  the  distance  from  the  foot-tip  to  the  support  surface 


in  the  direction  of  the  gravity  vector.  While  It  may  be  true  that  most 
proximity  sensors  would  not  necessarily  provide  this  kind  of  data,  it 


will  be  used  in  this  work  since  the  exact  form  of  the  data  will  vary 


significantly  with  the  physical  sensor  Implemented. 

From  the  terrain  database  obtained  through  Interactive  generation  of 
the  prismatic  terrain  model  as  described  in  Section  3.2,  it  is  known 


that  the  plane  equation  of  each  rectangular  surface  is 


AX  +  BY  +  CZ  +  D  =  0. 


(5.1) 


Let  the  foot  position  of  leg  i  in  the  earth- fixed  coordinate  system  be 


CXFi  YF1  ZFi]T, 


(5.2) 


and  let  a  point  which  is  a  vertical  projection  of  the  foot- tip  of  leg  1 


onto  the  rectangular  plane  along  the  -zF  direction  be 


E£pi  s  CXpi  Ypi  Zpi3T. 


(5.3) 


Then,  by  the  assumption  made  on  the  direction  of  proximity  sensing, 


Xpi  =  XF1, 
YP1  ■  YFi, 


(5.4) 


(5.5) 


and,  from  Eq.  (5.1), 


Zpi  *  - ( AXp*|  +  BYpi  +  D)/C. 


(5.6) 


The  output  of  each  proximity  sensor  Is  the  altitude  of  the  foot  of  leg  i 
above  the  support  surface,  F/^,  and  is  given  by 

FAi  *  zFi  "  zPi*  (5* 7) 

Figure  5.1  illustrates  the  method  for  simulating  each  proximity  sensor. 

5.3  Body  Motion  Planning 

Body  motion  planning  algorithms  specify  the  position  and  orientation 
of  the  vehicle  body  which  varies  according  to  the  operator's  velocity 
commands  and  the  terrain  conditions.  Many  of  the  algorithms  developed 
in  past  work  have  been  applicable  for  the  case  of  flat  level  terrain  [8] 
and,  2-0  planar  techniques  were  applied. 

In  this  section,  development  of  the  algorithms  Is  broken  into  three 
parts.  First,  the  technique  used  to  estimate  the  local  terrain  surface 
Is  presented.  Second,  the  coordinate  systems  used  to  specify  the  motion 
of  the  body  in  three  dimensions  are  defined.  Finally,  the  body 
regulation  plan  to  automatically  control  the  altitude  and  attitude  is 
described. 

5.3.1  Estimation  of  the  Support  Plane 

In  [12],  the  number  of  points  of  estimation  Is  limited  to  the  number 
of  legs  which  are  presently  In  contact  with  the  ground.  For  the  hexapod 
vehicle,  thus,  the  number  of  legs  in  the  support  phase  is  between  three 
and  six  during  locomotion,  which  often  gives  only  a  limited  set  of 
present  terrain  Information.  However,  through  the  use  of  the  proximity 
sensors,  the  number  of  points  of  estimation  becomes  equal  to  the  total 
number  of  legs  of  the  vehicle. 
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There  are  two  cases  to  be  considered  to  obtain  the  six  points  of 
estimation  for  a  hexapod  vehicle.  The  first  case  is  for  the  legs  In  the 
support  phase.  In  this  case  the  present  support  points  of  each  leg  may 
be  directly  used  in  estimation.  The  second  case  Is  for  the  legs  In  the 
transfer  phase.  The  transfer  trajectory  Is  divided  Into  three 
subphases:  lift-off,  transfer  forward,  and  placement,  which  will  be 
discussed  In  detail  in  Section  5.4.  When  a  leg  Is  in  the  placement 
subphase,  the  leg  proximity  sensor  detects  Its  approximate  touchdown 
point  on  the  ground.  This  touchdown  point  Is  used  in  the  estimation. 

It  may  be  noted  that  this  scheme  may  increase  terrain  adaptability  by 
controlling  the  body  altitude  and  attitude  before  the  leg  steps  onto  the 
ground.  When  the  transfer  leg  is  In  other  than  the  placement  subphase, 
its  last  support  point  is  used  for  estimation.  Figure  5.2  illustrates 
the  above  scheme. 

The  use  of  six  points  for  estimation  is  not  completely  arbitrary  and 
some  further  comments  may  serve  to  explain  this  choice.  As  proposed  In 
[12],  the  number  of  points  of  estimation  may  be  more  than  six  by 
Including  the  support  points  of  previous  locomotion  cycles.  However, 
the  adaptability  problem  Is  confined  to  the  local  terrain  conditions  In 
which  the  vehicle  Is  presently  traversing.  In  other  words,  after  the 
vehicle  has  already  passed  over  a  certain  point  of  the  terrain,  the 
terrain  Information  around  that  point  is  only  marginally  useful  for  the 
present  vehicle  motion.  Instead,  the  terrain  Information  ahead  of  the 
vehicle  Is  more  useful.  That  Is  why  the  vision  system  [26]  which 
provides  terrain  preview  Is  essential  for  operational  modes  other  than 
Close  Maneuvering.  Since  terrain  preview  is  not  provided  in  the  Close 
Maneuvering  Mode,  terrain  Information  may  be  obtained  from  the  support 
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leg  1  In  support  phase 

leg  j  In  transfer  phase  (other  than  placement  subphase) 
leg  k  In  placement  phase 


5.2.  Selection  of  Points  for  Each  Leg  Used  In  Estimating 
the  Support  Plane.  . 
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points  and  the  estimated  touchdown  points  as  measured  by  the  proximity 
sensors. 

The  estimation  technique  employed  Is  that  of  linear  regression  as 
described  In  [3].  The  only  change  made  In  the  equations  given  In  [3]  Is 
that  the  number  of  points  of  estimation  Is  fixed  to  six  as  previously 
described. 

The  foot  positions  are  usually  expressed  In  the  body  coordinate 
system  and  may  be  transformed  to  earth  coordinates  through  the 
homogeneous  transformation  given  In  Eq.  (3.2).  Using  the  appropriate 
support  points  or  measured  touchdown  points,  the  estimated  support  plane 
may  be  obtained  In  the  earth- fixed  coordinate  system  through  linear 
regression  as 

AX  +  BY  +  CZ  +  D  =  0  (5.8) 

where  [A  B  C]T  Is  a  unit  vector  which  points  outward  In  the  normal 
direction  from  the  plane.  The  estimated  plane  Is  situated  at  a  distance 
of  -D  along  the  normal  vector  from  the  origin  of  the  earth- fixed 
coordinate  system. 

While  the  legs  are  In  the  support  phase,  the  support  points  do  not 
change  with  respect  to  the  earth- fixed  coordinate  system.  The  support 
plane,  thus,  needs  to  be  estimated  only  when  at  least  one  of  the  six 
legs  begins  the  placement  subphase  at  which  time  the  proximity  sensor 
can  measure  the  touchdown  point  (next  estimated  support  point). 

5.3.2  Coordinate  Systems 

The  body  trajectory  Is  determined  not  only  by  the  operator's 
velocity  commands  but  also  by  the  terrain  conditions  Imposed  on  the 
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notion  of  the  body.  In  order  to  describe  the  body  motion  over  uneven 
terrain.  It  Is  useful  to  define  three  coordinate  systems:  the  earth- 
fixed  coordinate  system,  the  bo4y-f1xed  coordinate  system,  and  the 
terrain  coordinate  system.  The  earth  and  body  coordinate  systems  have 
been  defined  In  Chapter  3.  In  this  subsection,  the  terrain  coordinate 
system  and  the  homogeneous  transformation  matrices  between  the 
coordinate  systems  are  defined. 

Since  only  the  local  terrain  conditions  affect  the  vehicle  motion 
and  are  changing  while  the  vehicle  is  moving.  It  Is  convenient  to  define 
the  terrain  coordinate  system,  which  characterizes  the  local  terrain 
conditions,  as  a  moving  frame  with  respect  to  the  earth- fixed  coordinate 
system.  Furthermore,  the  terrain  coordinate  system  Is  defined  with 
respect  to  the  estimated  support  plane  discussed  In  the  previous 
section. 

The  origin  of  the  terrain  coordinate  system  (xj,yT,zx)  Is  located 
at  the  projection  of  the  body  center  of  gravity,  along  the  gravity 
vector  (-Z£  direction),  and  the  estimated  support  plane.  The  unit 
vector,  zt,  is  directed  normal  to  the  estimated  support  plane  and  Its 
components  In  the  earth- fixed  coordinate  system  are  given  by  Eq.  (5.8): 


E£t  *  [A  B  C]T. 


(5.9) 


The  unit  vector,  xj,  is  defined  to  be  In  the  direction  of  the  rotation 
axis  from  zg  to  zji  thus. 


ze  X  zt 

,  A  A 

l*E  X  *Tl 


t-  -  -  0]T, 
mm 


(5.10) 
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where  m  *  /  a2  +  b2 .  Finally,  the  unit  vector,  yy,  is  defined  to  give 
a  right-hand  set  of  unit  vectors.  With  this  definition  of  the  terrain 
coordinate  system,  yy  is  always  directed  up  the  slope.  Furthermore,  the 
slope  angle  Is  given  as: 

n  3  cos’Mzg  •  zy).  (5.11) 

If  n  3  0,  then  xy,  yy,  and  zy  are  forced  to  be  parallel  to  xg,  yg,  and 
zg,  respectively.  The  coordinate  systems  assigned  are  shown  In  Figure 
5.3.  Note  that  the  terrain  coordinate  system  changes  as  the  body  moves 
along  its  trajectory. 

The  homogeneous  transformation  matrix  H  from  body  coordinates  to 
earth  coordinates  has  been  defined  in  Eqs.  (3.2)  and  (3.3).  Let  the 
homogeneous  transformation  matrix  from  the  terrain  coordinates  to  earth 
coordinates  be  T.  It  has  the  following  form: 


where  Tr  Is  a  3X3  orientation  matrix  and  Ej>y  Isa  3X1  position  vector 
given  as: 

E£T  *  l>x  Py  Pz3T-  (5.13) 

From  the  foregoing  discussion  concerning  the  definition  of  the 
terrain  coordinate  system.  It  may  be  determined  that  Px  and  Py  are 
equal  to  the  x  and  y  components  of  the  position  vector  in  H, 
respectively.  By  substituting  Px  and  Py  Into  Eq.  (5.8),  Pz  is  obtained 


The  components  of  the  orientation  matrix  Tr  are  determined  by  the 
rotational  relationship  between  the  earth  and  terrain  coordinate  systems 
as  shown  In  Figure  5.4.  The  angle,  a,  between  xj:  and  xj  Is  derived  from 
Eq.  (5.10)  such  that 

a  =  tan-l(  —  ).  (5.15) 

Then,  Tr  Is  obtained  by  successive  rotations  about  the  z  and  x  axes 
[36]: 

Tr  =  Rot(z,a)Rot(x,n )  (5.16) 

"cosa  -slnocosn  slnaslnn 

=  slna  cosacosn  -cosaslnn  (5.17) 

0  slnn  cosn  J 

Let  the  homogeneous  transformation  matrix  from  the  body  coordinates 
to  terrain  coordinates  be  G.  Then  G  Is  simply  given  by 

G  -  T_1  H.  (5.18) 

The  three  coordinate  systems  and  the  three  transformation  matrices 
defined  In  this  section  will  be  utilized  to  define  the  body  and  leg 
trajectories  In  3-D  space  as  discussed  In  the  following  sections. 

5.3.3  Body  Regulation  Plan 

In  previous  work  for  the  OSU  Hexapod,  automatic  body  regulation 
algorithms  have  been  developed  to  control  the  bodty  attitude  and  altitude 
to  maintain  the  body  level  using  force  and  Inertial  sensors  [11]  or  to 
keep  the  body  parallel  to  and  at  a  constant  height  above  the  estimated 
support  plane  using  contact  Information  from  the  force  sensors  [12].  An 
algorithm  to  be  presented  In  this  section  deals  with  a  more  general  body 
regulation  plan  in  which  the  body  Is  not  always  parallel  to  the 
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estimated  plane  and  the  body  height  is  variable.  In  this  way,  this 
algorithm  may  allow  more  flexibility  in  body  attitude  and  altitude 
control  so  that  more  adaptability  to  terrain  irregularities  can  be 
achieved. 

The  body  velocity  for  full  si x-dgree-of- freedom  motion  consists  of 
the  translational  velocity,  j/,  and  the  rotational  velocity,  <o,  (with 
components  expressed  in  the  body  coordinate  system): 

1  a  Cvx  Vy  vz]T  (5.19) 

and 

oj_  *  [oj x  <*>y  (*>z  ]T,  (5.20) 

where  vx  is  the  longitudinal  speed;  Vy  is  the  lateral  speed;  vz  is  the 
vertical  speed;  wx  is  the  roll  rate;  oiy  is  pitch  rate;  uz  Is  the  yaw 
rate.  In  the  supervisory  control  scheme,  vx,  vy,  and  u>z  are  provided  by 
the  human  operator  and  vz,  ux,  and  Uy  are  the  velocity  components  to  be 
actively  controlled  by  the  control  software  to  achieve  the  desired  body 
attitude  and  altitude  according  to  the  body  regulation  plan. 

The  coefficients  In  the  equation  for  the  estimated  support  plane 
(Eq.  (5.8))  may  be  expressed  as  a  row  vector: 

E*  -  [A  B  C  D].  (5.21) 

Note  that  the  coefficients  are  written  with  respect  to  the  earth-fixed- 
coordinate  system.  Then,  may  be  simply  transformed  Into  body 
coordinates  [36]  by  postmultl plying  by  the  appropriate  homogeneous 
transform: 


The  components  of  the  unit  normal  vector  of  the  estimated  support  plane 


In  the  body  coordinate  system  are 
BzT  -  [A1  B*  C']. 


(5.23) 


Note  also  that  -O'  Is  the  distance  from  the  center  of  gravity  of  the 
body  along  Bzj  to  the  estimated  support  plane. 

For  body  attitude  control ,  It  is  necessary  to  define  the  desired 
body  angle,  rig,  which  is  measured  from  the  Z£  axis  on  the  plane  formed 
by  the  two  unit  vectors  Z£  and  zj.  The  angle  nD  is  positive  for 
counterclockwise  rotation  about  the  xy  axis.  Body  altitude  control  is 
accomplished  by  changing  the  distance  from  the  center  of  gravity  to  the 
estimated  support  plane  in  the  zy  direction. 

Figure  5.5  shows  the  body  attitude  and  altitude  regulation  plan 
adopted  in  this  simulation  work.  The  body  attitude  and  altitude  are 
related  to  the  slope  angle  n  through  piece-wise  linear  curves.  The 
quantities  and  n2  are  the  threshold  angles  of  the  slope  and  ho  is  the 
nominal  body  height  during  locomotion  over  flat  level  terrain.  The 
quantities  n  and  are  the  body  angle  and  height,  respectively, 
when  the  slope  angle  is  larger  than  All  these  values  may  be  set  by 
the  operator  before  the  vehicle  begins  to  move.  The  piece-wise  linear 


curves  are  expressed  as: 


_  nwax  '  nl  .  . 

n° "  „2  -„i  (n  -V  +ni 


for  0  <  n  <  n, 


for  <  n  < 


for  ng  <  n 


(5.24) 
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(a)  Attitude  Regulation 


(b)  Altitude  Regulation 


Figure  5.5.  Body  Regulation  Plan  for  Sloped  Terrain. 
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for  0  <  n  <  n. 


h0  “  *Vnin 


n2  _nl 


(n  -  n^)  +  hp 


for  n,  <  n  <  n. 


(5.25) 


for  n  g  <  n  . 


This  plan  Is  designed  to  keep  the  "belly"  of  the  vehicle  down  to  the 
ground  to  Increase  stability  over  sloped  terrain  and  to  Incline  the  body 
toward  the  slope  to  avoid  the  excessive  tilting  of  the  body  as  the  slope 


becomes  stiffer.  There  are  only  two  threshold  angles,  and  n2.  In 


this  plan.  However,  If  desirable  In  future  work,  the  number  of  the 
threshold  angles  may  be  rather  simply  Increased  to  more  than  two. 

From  the  above  regulation  plan,  the  desired  unit  normal  vector  of 


the  body  plane,  zq,  is  defined  In  Figure  5.5.  Since  zp  lies  on  the 
Ze-Zj  plane,  zq  may  be  obtained  by  rotating  ze  about  xy  by  the  angle  nQ* 
By  using  the  formula  of  a  rotation  about  an  arbitrary  axis  given  In  [36] 
along  with  Eq.  (5.10),  the  components  of  zp  in  the  earth  coordinates  are 


obtained  as: 


EZ0  -  [  ^  sionD  -  »«*,„  cosnD  ]T. 


(5.26) 


For  attitude  control.  It  Is  necessary  to  transform  Ezp  into  body 


coordinates  and  these  are  given  as  follows: 


Bzp  *  Hr-1  Ezp  *  [a  b  c]T. 


(5.27) 


Then,  the  rotation  axis  from  the  unit  normal  vector  of  the  present  bo4y 
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plane,  zg,  to  zg,  as  shown  in  Figure  5.6,  Is  given  by 


ZB  X  ZQ 

liB  X  id | 

C  -  7  7  o  ]T 
z  z 

[  kx  ky  kj  ]^, 


(5.28) 

(5.29) 

(5.30) 


where  z  *  /  a2  +  b*.  The  rotation  angle,  y,  is 
y  =  cos_1(zb  *  ZD)  =  cos-1(c). 


(5.31) 


Therefore,  attitude  control  may  be  done  by  rotating  zg  about  the 
rotation  axis  k  by  y  in  an  appropriate  manner.  For  a  physical  system. 

It  is  not  possible  to  move  instantaneously  by  the  angle  y.  In  order  to 
achieve  a  smooth  transition,  the  following  control  law,  which  gives  an 
exponential  response.  Is  defined  such  that 

v*-KAy.  (5.32) 

where  Ka  Is  the  Inverse  of  the  time  constant  for  the  motion  .  From 
Eqs.  (5.30)  and  (5.32),  the  rotational  vector  y  k  may  be  decomposed  into 
two  components  In  the  body  coordinate  system: 

Y  t  -  -Ka  kx  t  xg  -  Ka  ky  y  yg.  (5.33) 

The  x  and  y  components  may  be  related  to  the  roll  rate,  ux,  and  the 
pitch  rate,  wy,  respectively,  with  the  following  result: 

wx  *  -Ka  kx  y  (5.34) 

wy  *  -Ka  ky  y •  (5.35) 
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For  altitude  control,  the  present  body  height,  h.  Is  obtained 
from  Eq.  (5.22): 


h  *  -O'. 


(5.36) 


Also,  the  desired  body  height,  hQ,  is  given  In  Eq.  (5.25).  In  the  same 


manner  as  above,  the  control  law  for  altitude  Is  defined  as 


vz  =  -K/v(  hp  -  h). 


(5.37) 


Note  that  the  above  altitude  control  law  Is  only  valid  under  the 


assumption  that  the  angle  between  the  body  and  the  estimated  support 


plane  is  small. 


The  above  discussion  about  the  automatic  body  regulation  algorithm 
may  be  summarized  In  a  flow  chart.  That  is  shown  in  Figure  5.7. 


In  real-time  operation,  the  control  program  is  executed  In  an 


Incremental  time.  At,  which  Is  called  the  computation  cycle  time.  The 


six  body  velocity  components  computed  as  above  cause  a  differential 
change  In  H  by  At.  The  differential  change  In  H,  dH,  Is  defined  as 
follows  [36]: 


dH  »  H  Ha  # 


(5.38) 


where  Ha,  which  Is  called  the  differential  translation  and  rotation 


transform.  Is  given  as: 
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(5.39) 


m 


Compute  attitude  and  altitude 
velocity  components  of  body 


Figure  5.7.  Flow  Chart  for  the  Body  Regulation  Algorithm. 


dx  2  Vx*At 
dy  h  Vy*At 
dz  2  Vz*At 
6  x  -  (lix'b  t 

6y  2  Uly’At 

<5 Z  -  «z*At. 

Thus,  H  is  updated  each  computation  cycle  as 

H  H  +  dH.  (5.4] 

However,  Eq.  (5.41)  may  not  guarantee  orthogonality  of  the  orientation 
part  (Hr)  of  the  updated  H  matrix  due  to  the  differential  change.  In 
order  to  orthogonal Ize  the  updated  H  matrix,  the  Gram-Schmlt 
orthogonal Izatl on  process  [39]  was  employed. 

5.4  Leg  Motion  Planning 

Leg  motion  planning  algorithms  are  designed  to  generate  the  foot 
trajectory  according  to  the  change  of  the  kinematic  cycle  phase  and 
to  choose  the  desired  foothold  for  the  transfer  leg.  The  change  of 
kinematic  cycle  phase  and  the  foothold  selection  mainly  depend  on  the 
body  velocity  and  the  Interaction  of  the  support  plane  and  constrained 
working  volumes  for  the  legs. 

From  Eq.  (2.2),  the  kinematic  cycle  phase,  may  be  updated  every 
computation  cycle  time,  At,  as  follows: 

♦  ♦  C#  +  T^mod  1  • 


(5.4 


Then  the  leg  phase  variable  of  leg  1,  Is  updated  through  use  of 
Eq.  (2.6).  The  updated  kinematic  cycle  phase  and  leg  phase  variables 
determine  the  the  state  of  leg  1,  support  phase  or  transfer  phase,  and 
synchronize  the  leg  stepping  events  In  order  to  Implement  wave  gaits. 

In  this  simulation  work,  the  proximity  sensor  Is  used  to  specify  the 
touchdown  point  of  the  transfer  leg  which  is  exactly  the  contact  point 
on  the  terrain  surface.  On  real  terrain,  however,  the  touchdown  point 
measured  by  the  proximity  sensor  may  not  actually  be  on  the  ground  due 
to  measurement  errors.  To  ensure  foot  contact,  contact  sensors  (often 
Implemented  by  force  sensors)  should  be  used  at  the  end  of  transfer 
phase. 

The  following  subsections  discuss  the  problems  related  to  foot 
trajectory  generation  over  uneven  terrain  In  3-D  space.  The  reference 
position  which  Is  useful  In  selecting  the  foothold.  Is  first  defined  to 
be  on  the  center  line  of  the  constrained  working  volume.  The  support 
trajectory  Is  then  computed  based  on  the  foothold  and  body  velocity. 
Finally,  the  transfer  trajectory  Is  defined,  again  based  on  the  foothold 
and  also  the  slope  of  the  terrain. 

5.4.1  Reference  Position 

In  order  to  plan  the  trajectory  of  a  supporting  foot,  a  nominal  foot 
position,  near  the  center  of  the  constrained  working  volume  through 
which  the  foot  trajectory  Is  predicted  to  pass.  Is  defined.  This  point 
Is  called  the  reference  position.  The  reference  position  Is  also  used 
to  determine  the  desired  foothold  and  this  will  be  discussed  In  Section 


T-.V.TvT'V  4.T-  *•(.  "W  T  i'  ti 


For  locomotion  over  flat  level  terrain  with  the  body  parallel  to  the 
support  plane,  the  reference  position  is  chosen  to  be  located  at  the 
center  of  the  constrained  working  volume,  a  point  which  Is  on  the 
support  plane  as  shown  In  Figure  5.8a.  In  this  case,  the  nominal  body 
height,  hQ,  is  equal  to  the  z-component  of  the  center  of  the  constrained 
working  volume,  Zci*  in  body  coordinates.  By  planning  the  support 
trajectory  to  pass  through  the  center  of  the  constrained  working  volume, 
each  leg  may  more  fully  utilize  its  constrained  working  volume  while  In 
the  support  phase. 

For  locomotion  over  sloped  terrain  as  shown  in  Figure  5.8b,  the  body 
may  not  always  be  parallel  to  the  support  plane.  Also,  the  center  of 
the  constrained  working  volume  may  not  be  located  on  the  support  plane. 
Therefore,  In  order  to  Include  the  more  general  case  for  locomotion  over 
uneven  terrain,  the  reference  position  of  a  leg  is  defined  as  a  point 
inside  its  constrained  working  volume  which  is  at  the  Intersection  of 
the  estimated  support  plane  and  the  "vertical"  line  which  passes  through 
the  center  of  the  constrained  working  volume  in  the  zg  direction. 

The  reference  position  for  a  leg,  R,  as  shown  In  Figure  5.9  may  be 
computed  in  the  following  manner.  The  point  C  denotes  the  center  of  the 
constrained  working  volume,  and  R  Is  at  the  Intersection  of  the  center 
line  and  the  estimated  support  plane.  Let  the  position  vector  of  C  In 


body  coordinates,  be 
\  *  [Xc  YC  ZC]T. 

The  quantities  Xq,  Yq>  and  Zc  are  given  In  Section  4.2. 
position  vector  of  R  In  body  coordinates,  be 

%  •  CXR  Y„  Zr]T. 


(5.43) 


Also,  let  the 


(5.44) 
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(a)  Foot  Reference  Positions  over  Flat  Level  Terrain 
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(b)  Foot  Reference  Positions  over  Sloped  Terrain 


Figure  5.8.  Foot  Reference  Positions  over  Level  and  Sloped  Terral 


From  the  foregoing  discussion  concerning  the  definition  of  the  reference 
position,  Xr  and  Yr  are  given  as  follows: 


XR  =  XC  (5.45) 

Yr  =  YC  .  (5.46) 

From  Eq.  (5.22),  the  equation  for  the  estimated  support  plane  in  body 
coordinates  is 

A'X  +  B'Y  +  C'Z  +  O'  =  0.  (5.47) 

Thus,  by  substituting  Eqs.  (5.45)  and  (5.46)  into  Eq.  (5.47),  Zr  is 
obtained  as 

Zr  ■  -(A'Xr  +  B'Yr  +D')/C‘.  (5.48) 


5.4.2  Support  Phase 

In  the  support  phase,  the  basic  problem  is  to  determine  the  updated 
values  of  the  positions  and  rates  of  each  supporting  foot  in  the 
body- fixed  coordinate  system  as  the  body  moves  through  the  desired 
trajectory.  Since  the  foot  positions  in  earth  coordinates  are  assumed 
to  be  constant  during  the  support  phase,  these  known  values  may  be 
transformed  to  body  coordinates  through  the  continuously  updated  H 
matrix  as  given  In  [12]: 

(5.49) 

The  approach  to  determining  the  foot  rates  Is  different  from  that  in 
[12].  Using  Eqs.  (5.19)  and  (5.20),  by  elementary  physics,  the  foot 
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velocity  of  leg  1  relative  to  the  body  motion,  Vj,  is  given  by 

v  1  =  -  v  -  w  X  (5.50) 

In  matrix  form,  Eq.  (5.50)  is 


-“zYF1  +  tOyZpi 
t»zxF1  -  wxZfi 
-uyXp(  +  ux^Fi  * 


(5.51) 


5.4.3  Transfer  Phase 

In  the  transfer  phase,  the  problems  to  be  addressed  include  those  of 
foothold  selection  and  the  determination  of  the  positions  and  rates  of 
the  foot  of  leg.  The  transfer  trajectory  is  defined  based  on  the 
foothold  and  terrain  variations,  using  the  transfer  phase  variable  of 
each  leg,  ,  which  has  been  defined  in  Section  2.2.2. 

5.4.3. 1  Desired  Foothold 

The  desired  foothold  of  a  transfer  leg  Is  selected  at  a  point  on  the 
surface  of  the  constrained  working  volume  such  that  Its  support 
trajectory  is  predicted  to  pass  through  the  foot  reference  position. 

For  computational  purposes,  It  Is  assumed  that  the  foot  velocity  for  the 
support  trajectory  would  be  equal  to  the  foot  velocity  at  the  reference 
position  for  the  present  body  velocity. 

While  the  reference  position  Is  valuable  In  selecting  a  foothold  as 
well  as  defining  the  support  trajectory.  It  should  be  noted  that  the 
actual  support  trajectory  may  not  pass  through  the  reference  position. 
This  results  because  of  changes  in  the  body  velocity  from  that  used  in 
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computing  the  foothold.  Also,  even  with  constant  body  velocity,  the 
trajectory  will  usually  not  be  a  straight  line  as  assumed.  Figure  5.10 
illustrates  the  above  in  3-0  space. 

In  general,  for  constant  body  velocity,  the  actual  support 
trajectory  will  be  an  arc  of  a  circle  as  shown  in  Figure  5.10.  In  this 
work,  the  desired  foothold,  which  is  the  intersection  point  of  the  foot 
trajectory  with  the  constrained  working  volume,  is  computed  by 
approximating  the  circular  foot  trajectory  with  a  straight  line.  This 
will  simplify  the  computation  considerably. 

The  desired  foothold  is  computed  as  follows.  First,  by  using  Eqs. 
(5.44)  and  (5.51),  the  predicted  foot  velocity,  v^,  at  the  reference 
position  with  the  present  body  velocity  is  obtained: 

4  *  [XR  Yr  Zr3t.  (5.52) 

Then  by  taking  a  point,  Pq,  outside  the  constrained  working  volume  in 
the  -v_  direction  and  connecting  it  to  the  reference  position,  Pr,  with 

K 

a  straight  line,  there  is  an  intersection  point.  Pint.  on  the  edge  of 
the  constrained  working  volume  as  shown  in  Figure  5.11a.  The  components 
(X0,Y0,Z0)  and  (Xint,Yint,Zi„t)  denote  the  position  of  Pq  and  P1nt, 
respectively,  in  body  coordinates. 

To  simplify  the  computation,  P^t  is  first  computed  in  the  Xr-Yr 
plane.  For  example,  Pin*  as  shown  in  Figure  5.11b  is  given  by 

Y2  -  Y0 

xint  s  Xq  (X R  -  *)>  <5.53) 

Y1nt  -  Y2.  (5.54) 

In  the  same  manner,  the  Intersection  points  for  segments  crossing 
other  edges  may  be  computed.  This  method  is  employed  in  computer 


Figure  5.10.  Relationship  between  the  Predicted  and  Actual 
Support  Trajectory. 
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graphics  for  clipping  algorithms  [39] . 

The  values  for  X-fn^  and  Yjnt  given  by  Eqs.  (5.53)  and  (5.54)  may  not 
be  correct,  because  they  are  obtained  only  in  the  Xg-Yg  plane.  In  other 
words,  they  may  be  outside  the  constrained  working  volume  In  the  zg 
direction.  In  order  to  check  this,  the  time,  tj.,  to  go  from  Pr  to  Pjnt 
using  X^nt  and  Yint  of  Eqs.  (5.53)  and  (5.54)  Is  computed  as  follows: 

lx1nt  “  XR|  lYint  "  yrI 


Also,  the  time,  t2,  to  go  from  Pr  to  the  edge  of  the  constrained  working 
volume  In  the  zg  direction  needs  to  be  computed.  The  time  t2  is  obtained 
from  the  z-dlmenslon  of  the  constrained  working  volume  and  is  given  as: 


|Zmax  *  Zpl 


for  Zr  >  0 


(5.56) 


|Zm1n  "  zr| 


for  Zp  <  0 


where  Zmax  *  Zqi  +  dz<|/2  and  Zmin  ■  Zqi  -  dz-(/2  which  are  defined  in 
Section  4.2.  Thus,  if  <  t2,  X<{nt  and  Y-jnt  given  by  Eqs.  (5.53)  and 
(5.54)  are  correct  and  Zjnt  * s  simply 


z1nt  *  Zr  -  ZR-ti. 


(5.57) 


If  >  t2,  then 


x1nt  *  XR  -  XR*^ 
Y1nt  *  YR  “  YR**2 
Zlnt  *  Zr  -  ZR-t2 


(5.58) 


Thus,  the  the  components  of  the  desired  foothold  of  leg  i,  B£p^»  are: 

b£q^  *  £x1nt  Yint  Zint]T 
and,  in  earth  coordinates, 

E*01  -HV 

5. 4. 3. 2  Foot  Transfer  Trajectory 

The  transfer  trajectory  Is  divided  into  three  subphases:  lift-off, 
transfer  forward  and  placement.  The  trajectories  for  lift-off  and 
placement  may  be  generated  by  moving  the  foot  straight  up  or  down  along 
the  gravity  vector  from  the  lift-off  point  or  to  the  desired  foothold. 
Let  the  values  of  $  at  the  end  of  the  lift-off  subphase  and  at  the 
beginning  of  the  placement  subphase  be  ^  and  <t,p,  respectively,  which 
are  defined  as  constants.  Then,  the  subphase  to  which  leg  1  belongs  Is 
determined  as  follows: 

0  <  <t»T1  <  4>l  leg  11n  lift-off  (5.61) 

$L  <  ^  <  $p  leg  1  In  transfer  forward  (5.62) 

<t>p  <  <t>Ti  <  1  leg  1  In  placement.  (5.63) 

During  the  lift-off  subphase,  the  total  time  left,  Tpu,  until  the 
foot  reaches  the  foot  lift  height,  hp,  which  Is  preset  to  be  a  constant. 
Is 

TFL1  *  UL  -  ♦t1)tr,  (5.64) 

where  tr  3  (1  -  @)t,  which  Is  the  total  transfer  time.  In  the  same 
manner,  the  time  left  to  the  end  of  the  transfer  forward  subphase,  Tpj{, 


(5.59) 

(5.60) 


and  the  time  left  until  foot  placement  during  the  placement  subphase, 
Tppi ,  are  obtained  as  follows: 

tFT1  3  4p  -  *Ti  )tr  (5. 

tFP1  s  (1  -  4>t1*tr.  (5. 

When  leg  1  begins  a  new  subphase,  the  foot  position  at  the  end  of 
each  subphase,  Is  computed  as  described  in  the  following 

paragraphs.  The  movement  from  the  present  foot  position  to  the  end 
point  is  made  to  be  linear  over  time  In  the  earth- fixed  coordinate 
system  during  each  subphase.  The  following  vector  equation  may  be 
applied  to  all  three  subphases  to  update  the  foot  position  over  an 
increment  of  time,  at: 

E%,  *  E%,  ♦  <Ei£,  -  E£f,)At/Tf.  (5. 

where  T-j  Is  one  of  Tpu,  Tp-p^,  and  Tppf. 

The  end  points  of  each  subphase  are  computed  in  the  following  way 
Let  the  foot  position  of  leg  1  when  ^  »  0  be 

E£T1  a  £XT1  YT1  ztOt-  <5. 

Then,  from  Eq.  (5.68),  the  end  point  for  the  lift-off  subphase  Is 


E%1  *  CXti  Yji  ZTt  +  hp]T. 


Let  the  desired  foothold  obtained  from  Eq.  (5.60)  be 


e£01  *  £*oi  ydi  zdi3t. 


then  the  end  point  for  the  transfer  forward  subphase  Is 


e2  -  C*01  *01  Zqi  +  HfF. 


1 1 %  » %  ji*fcjj% 


*>J 


However,  In  order  to  avoid  foot  contact  with  rough  terrain  during  the 
transfer  forward  subphase,  the  proximity  sensor  Is  used  to  keep  the  foot 
altitude  to  be  hp  above  the  terrain  In  the  zg  direction.  Therefore,  the 
x  and  y  components  of  Ejjpj.  XF1  4nd  *F1»  may  be  updated  using 
Eqs.  (5.67)  and  (5.71),  but  the  z  component,  Zpf,  Is  updated  using  foot 
altitude  Information  given  by  Eq.  (5.7)  as  follows: 


ZF1  ♦  ZF1  +  (hp  -  FA1). 


(5.72) 


During  the  placement  subphase,  the  end  point  Is  measured  by  the 
proximity  sensor  and  It  Is  given  by  Eq.  (5.3): 

E%1  *  {s-n) 

From  Eq.  (5.67),  the  foot  velocity  In  earth  coordinates  Is 

E11  2  ^EfiEi  "  E£p^  /  T1*  (5.74) 

For  the  transfer  forward  subphase,  the  x  and  y  components  of  Evj ,  xF1 
and  Ypj,  are  obtained  from  Eq.  (5.74),  but  the  z  component,  Zpi,  Is 
computed  from  Eq.  (5.72)  as 

Zpi  a  (hp  -  FAj)  /  at.  (5.75) 

The  position  and  velocity  vectors  may  be  transformed  Into  body 
coordinates  as  follows: 

B£pi  «  H-l  (5.76) 

Bvi  -  HP-1  .  (5.77) 

Figure  5.12  shows  the  transfer  trajectory  generated  according  to  the 
algorithm  as  discussed  above. 
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5.5  Adjustment  of  the  Constrained  Working  Volume 

The  constrained  working  volume  for  each  leg  as  discussed  In  Section 
4.2,  may  be  changed  somewhat  by  changing  Its  position  and  dimensions 
Inside  the  original  working  volume.  This  flexibility  may  be  utilized  to 
Increase  the  stability  of  the  vehicle  for  locomotion  over  uneven 
terrain.  Two  schemes  to  Increase  stability  are  discussed  In  the 
following  paragraphs. 

In  wave  gaits  for  a  hexapod  vehicle.  It  may  be  observed  that  the 
stability  margin  mainly  depends  on  the  foot  positions  of  the  middle  legs 
for  straight- line  locomotion,  especially  In  the  case  of  the  tripod  gait. 
This  observation  leads  to  a  scheme  to  adjust  the  position  of  the 
constrained  working  volume  of  the  middle  legs  when  walking  over  sloped 
terrain. 

For  example.  Figure  5.13  shows  the  change  of  the  stability  margin  by 
moving  the  position  of  the  constrained  working  volume  of  the  middle  leg, 
for  straight-line  locomotion  with  the  tripod  gait  over  sloped  terrain. 

In  Figure  5.13a,  the  cross  sections  of  each  constrained  working  volume 
with  the  slope  are  expressed  as  AB  for  the  front  leg,  CD  for  the ‘middle 
leg,  and  EF  for  the  rear  leg.  Also,  Figure  5.13b  shows  the  vehicle  on 
the  X£-Y£  plane  and  the  stability  margin  MK  before  changing  the  position 
of  CD.  If  CD  Is  moved  down  the  slope  to  CD1,  then  the  stability  margin 
Is  MK1  as  shown  In  Figure  5.13b.  Note  that  the  length  of  MK1  Is  greater 
than  that  of  MK. 

Though  the  above  discussion  Is  specific  to  straight-line  locomotion 
for  the  tripod  gait  over  simple  sloped  terrain.  It  Is  also  applicable  to 
arbitrary  motion  of  the  vehicle  for  other  gaits  over  uneven  terrain. 


Thus,  the  position  of  the  constrained  working  volume  of  a  middle  leg  may 


be  dynamically  changed  according  to  the  local  terrain  slope  to  Increase 
stability.  The  position  change  Is  made  in  the  xg  direction  only,  since 
the  magnitude  of  the  y  position  of  each  constrained  working  volume  may 


be  simply  set  to  a  maximum  in  order  to  Increase  the  stability  margin. 


As  shown  in  Figure  5.14,  the  points  C  and  R  are  the  center  and  the 


reference  position  of  the  constrained  working  volume  for  a  middle  leg. 


respectively,  before  adjustment.  Point  N  is  the  projection  of  C  onto 
the  Xg-Yg  plane  in  the  zg  direction.  The  points  C‘  and  R‘  are  the  new 
center  and  the  new  reference  position,  respectively,  after  adjustment 
such  that  N1  is  the  projection  of  N  along  the  gravity  vector  onto  the 
estimated  support  plane.  Let  the  position  vector  of  C  of  middle  leg  1, 


which  is  given  in  Section  4.2,  be 


\  s  £xCi  Yci  Zci 3T* 


(5.78) 


Then,  the  position  vector  of  N  in  body  coordinates  is 


%  *  CXCi  YCi  0]T, 


(5.79) 


and,  in  earth  coordinates,  it  Is 


>  CXN  Y„  Z„]T. 


(5.80) 


Let  the  position  vector  of  N'  in  earth  coordinates  be 


=  txN'  V  ZN'^' 


(5.81) 


From  Eqs.  (5.80)  and  (5.81), 


V  *  XN 


(5.82) 


I 


E 
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Thus,  in  body  coordinates,  the  position  vector  of  N'  is 

BV  =  H_1  %>  =  Ca  b  c]T,  (5.85) 

and  the  new  center  is  obtained  by  replacing  the  x  component  of  of 


Eq.  (5.78)  with  that  of  Bj^,  0f  Eq.  (5.85): 
®Eq.  a  Ca  Yci  ZciF. 


(5.86) 


The  corresponding  reference  position  R'  is  determined  by  Eqs.  (5.43) 
through  (5.48). 

An  observation  has  been  made  in  Section  4.2  that  the  stability  of 
vehicle  motion  Increases  by  moving  the  soft  limits  imposed  by  the 
constrained  working  volumes  further  away  from  the  center  of  gravity.  In 
other  words,  stability  may  Increase  by  changing  the  reference  position 
to  be  further  away  from  the  center  of  gravity.  However,  since  the 
position  of  the  constrained  working  volune  for  each  leg  is  defined  to  be 
located  at  the  corner  of  Its  working  volume  as  mentioned  in  Section  4.2, 
the  only  way  to  change  the  reference  position  Is  to  reduce  the  size  of 
the  constrained  workl ng  volume. 

If  the  body  speed  Is  slow,  the  leg  does  not  need  the  large 
constrained  working  volume  since  the  support  trajectory  for  a  given 
support  period  may  be  short.  Thus,  for  slower  speeds,  the  size  of  the 
constrained  working  volume  mqy  be  made  smaller,  which  In  turn  causes  the 
reference  position  to  move  further  away  from  the  center  of  gravity. 


Sfl 


This  approach  may  increase  stability  considerably,  especially  for 
locomotion  over  terrain  with  a  stiff  slope,  because  the  body  velocity  Is 
usually  commanded  to  be  very  slow  in  this  case. 

The  change  of  dimensions  of  the  constrained  working  volume  of  a  leg 
should  be  made  while  the  leg  Is  in  the  transfer  phase.  As  mentioned  in 
Section  5.4,  the  desired  foothold  of  the  transfer  leg  is  specified  to  be 
on  the  boundary  of  the  constrained  working  volume  such  that  its 
predicted  support  trajectory  passes  through  the  center  of  its 
constrained  working  volume.  If  the  dimensions  are  changed  after  the 
desired  foothold  is  chosen.  It  Is  possible  for  the  support  point  of  the 
leg,  while  in  the  support  phase,  to  go  outside  the  constrained  working 
volume  due  to  body  velocity  changes.  This  complicates  the  algorithm  for 
determining  the  optimal  period  as  discussed  in  Chapter  4,  and  should  be 
avoided. 

The  change  of  the  constrained  working  volume  is  made  on  the  x  and  y 
dimensions  only,  since  the  z  component  of  the  foot  velocity  is 
relatively  small  compared  to  the  x  and  y  components  during  normal 
operation  of  the  vehicle.  Thus,  for  simplicity,  dxj  *nd  dyj  of  the 
constrained  working  volume  of  leg  1  are  set  to  vary  linearly,  between 
minimum  and  maximum  values,  with  the  predicted  foot  velocity  given  by 
Eq.  (5.52).  The  equations  for  the  change  of  dxi  and  dyi  are  given  as 


follows: 


(dxmax  "  dxm1n) 

dxi  *  dxm1n  +  (v^^  -  v^jn)  "  vxm1n) 

(dymax  “  dymln) 

dj'1  "  djn”1n  *  (W  -  w  (IYrI  ' Vy"1"’ 


(5.87) 


(5.88) 


where  d^-jp,  dxmax,  <|ymint  dymax,  vxm1n»  vxmax»  vym1n*  an<*  vymax  are 
constants. 

It  is  possible  to  obtain  minimum  allowable  values  for  dx-f  and  dyj 
for  straight-line  locomotion  over  flat  level  terrain  in  order  to  avoid 
vehicle  slow-down  (see  Section  4.4).  These  values  are  a  function  of  the 
body  velocity,  and  the  theoretical  curves  obtained  in  the  following 
analysis  will  aid  in  setting  reasonable  values  for  d^n,  dxmax,  dy^- n , 
etc.  For  example,  when  the  vehicle  moves  with  the  constant  speed  vx  in 
the  xg  direction  only  and  wave  gaits  are  used,  then  the  following 
equations  hold  [8]: 

X  3  tvx  ,  (5.89) 

dXi  3  gx  ,  (5.90) 

and 

*  (1  -  e)x  .  (5.91) 


However,  as  discussed  in  Section  4.4,  in  order  to  avoid  slow-down,  the 
transfer  time  should  be  greater  than  the  transfer  time  threshold: 


(5.92) 


From  Eqs.  (5.89)  through  (5.92),  values  for  dx-f  which  avoid  slow-down 
may  be  computed: 


»  r^T  TRth  **  •  (5-93» 

The  results  for  dyi  are  similar  for  pure  lateral  motion.  Figure  5.15 
gives  a  graph  of  the  minimum  allowable  values  for  dx-f  as  a  function  of 
vx  for  three  different  values  of  3  when  tBth  * 
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5.  Minimum  X-DImenslon  of  the  Constrained  Working  Volume 
as  a  Function  of  the  Forward  Velocity  (vx)  and  Duty 
Factor  (e)  In  Order  to  Prevent  Slow-down  when  the 
Minimum  Leg  Transfer  Time  (tr)  Is  0.5  sec. 


5.6  Summary 

Algorithms  developed  for  motion  planning  over  uneven  terrain,  which 
allow  implementation  of  the  Close  Maneuvering  Mode  of  operation  for  the 
ASV,  were  presented  in  this  chapter.  These  algorithms  make  use  of 
proximity  sensors  for  each  leg  and  these  were  also  described  in  detail. 
Since  in  this  simulation  work  the  actual  foothold  on  the  terrain  is 
exactly  measured  by  proximity  sensors,  there  is  no  distinction  between 
these  and  contact  sensors.  However,  for  the  vehicle  Itself,  contact 
sensors  shoud  be  used  to  ensure  foot  contact.  The  proximity  sensors 
provide  local  terrain  Information  before  the  legs  touch  the  ground. 

This  enables  the  vehicle  to  adapt  to  terrain  irregularities  by  making 
soft-contact  on  the  terrain  surface. 

The  estimation  algorithm  for  the  support  plane  has  been  developed  in 
order  to  make  use  of  proximity  sensors  for  each  leg  by  defining  the 
rectangular  transfer  trajectory.  The  plane  of  support  is  estimated  by 
using  the  support  points  and  the  predicted  footholds  through  linear 
regression.  Due  to  the  limited  number  of  estimation  points,  sometimes, 
the  estimation  may  result  in  large  error,  especially  in  the  region  where 
the  slope  changes  abruptly.  If  It  Is  assumed  that  the  vehicle  speed  Is 
relatively  slow  and  the  terrain  Is  relatively  smooth  In  the  Close 
Maneuvering  Mode,  the  estimation  error  may  not  cause  apy  trouble  to  the 
vehicle  motion. 

In  order  to  describe  the  boty  motion  over  uneven  terrain,  three 
coordinate  systems  have  been  defined:  the  earth-fixed  coordinate 
system,  the  body- fixed  coordinate  system,  and  the  terrain  coordinate 
system.  The  terrain  coordinate  system  has  been  defined  as  a  moving 
frame  with  respect  to  the  earth- fixed  coordinate  system,  which 
characterizes  the  local  terrain  conditions. 


Using  the  three  coordinate  systems,  the  automatic  body  attitude  and 
altitude  control  scheme  has  been  presented  in  a  mathematically 
straightforward  manner  according  to  a  simple  body  regulation  plan.  The 
body  regulation  plan  has  been  designed  in  such  a  way  that  the  body 
attitude  and  altitude  can  be  related  to  the  slope  angle  through 
piece-wise  linear  curves  and  the  desired  unit  normal  vector  of  the  body 
plane  lies  between  the  gravity  vector  and  the  normal  vector  of  the 
estimated  support  plane. 

The  support  and  transfer  trajectories  for  the  foot  of  the  leg  have 
been  defined.  Especially,  the  transfer  trajectory  is  generated  by  using 
the  proximity  sensor  in  order  to  avoid  foot  contact  with  the  terrain. 

The  intersection  point  of  the  estimated  support  plane  with  the  center 
line  of  the  constrained  working  volume  of  each  leg  has  been  chosen  as  a 
reference  position  to  select  Its  desired  foothold,  while  in  the  transfer 
phase.  The  desired  foothold  is  selected  at  a  point  on  the  surface  of 
the  constrained  working  volume  by  approximating  the  circular  foot 
trajectory  during  the  support  phase  with  a  straight  line.  This 
approximation  results  In  simple  computation. 

Finally,  in  order  to  Increase  the  stability  margin  over  uneven 
terrain,  two  schemes  have  been  presented:  one  Is  for  the  change  of  the 
position  of  the  constrained  working  volume  for  each  of  the  middle  legs 
and  the  other  Is  for  the  change  of  dimensions  of  the  constrained  working 
volume  for  each  of  all  legs.  The  position  of  the  constrained  working 
volume  of  the  middle  leg  Is  changed  in  the  xg  direction  only  toward  the 
gravity  vector,  while  In  the  transfer  phase.  The  size  of  the 
constrained  working  volume  Is  adjusted  according  to  the  predicted  foot 
velocity  during  the  transfer  phase  without  causing  the  vehicle  to 
unnecessarily  slow  down.  These  schemes  result  from  the  flexibility  In 
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Chapter  6 
CONTROL  SOFTWARE 


6.1  Introduction 

Recently,  a  control  software  architecture  for  the  Hexapod  vehicle 
has  been  proposed  by  Orln  and  Pugh  [41],  In  which  the  control  program  Is 
partitioned  Into  functional  blocks,  or  subtasks,  for  the  multicomputer 
system.  Since  communication  between  the  blocks  Is  still  under 
Investigation,  the  software  structure  of  the  control  program  for  the  ASV 
developed  In  this  work  was  based  on  the  previous  control  programs  for 
the  OSU  Hexapod  [11,12]  and  modularized  Into  the  several  routines.  Each 
routine  consists  of  several  subroutines  which  will  be  discussed  In 
Section  6.2. 

In  Section  6.3,  evaluation  of  the  control  algorithms  developed  In 
Chapter  5  will  be  presented.  Goodness  of  the  algorithms  Is  measured  by 
the  stability  margin  when  the  vehicle  moves  forward  over  sloped 
terrain. 

6.2  Software  Organization 

There  are  six  functional  blocks  defined  In  this  simulation  software 
as  shown  in  Figure  6.1  with  slight  jnodlfl cation  of  the  control  task 
partitioning  of  the  OSU  Hexapod  contol  program  version  3.0  given  In 


The  servo  control  functional  block  which  Is  essential  to  control  the 
vehicle  at  the  servomechanism  level  Is  replaced  by  the  Vehicle 
Simulation  block. 

Figure  6.2  shows  the  structured  flow  chart  [42]  for  the  control 
software  for  the  ASV  In  which  the  solid  lines  Indicate  the  subroutine 
calls  and  the  dashed  arrows  Indicate  file  transfer  between  two  modules. 
For  example,  module  BODY  Invokes  module  LEGCORD  and  LEGCORD  Is 
subordinate  to  BODY.  Each  module  In  Figure  6.2  represents  the  actual 
routine  Implemented  and  corresponds  to  each  functional  block.  The 
Terrain  Simulation  block  consists  of  two  Independent  modules  which  pass 
the  database  for  the  prismatic  terrain  model  and  the  viewing  angle  to 
the  control  software.  The  software  Is  Implemented  In  PASCAL  except  the 
Interface  routines  for  the  graphics  device  and  the  joystick  on  the 
PDP-11/70  minicomputer. 

6.2.1  Man-Machine  Interface 

The  Man-Machine  Interface  block  provides  the  Interface  between  the 
human  operator  and  the  control  program  for  the  Interactive  change  of  the 
related  parameters  for  the  vehicle  motion  using  the  keyboard  and  for  the 
velocity  commands  through  a  three-axis  joystick.  Before  starting  to 
move  the  vehicle,  the  operator  can  Interactively  change  the  duty  factor 
for  the  desired  wave  gaits,  the  maximum  height  of  footllft  for  the  foot 
transfer  trajectory  and  then  enter  a  command  to  Initialize  the  vehicle 
position  through  the  keyboard. 

When  the  Initialization  Is  finished,  another  keyboard  command  Is 
entered  to  start  motion.  After  that,  the  vehicle  motion  Is  directed  by 


the  joystick  velocity  commands  until  a  keyboard  command  which  Is  for  the 
vehicle  to  halt  Is  entered.  Figure  6.3  depicts  the  exact  relationship 
between  the  three  components  of  the  body  velocity  and  the  axes  of  the 
joystick  [8]. 

6.2.2  Body  Set-Point  Generation 

This  block  generates  body  position  and  velocity  set-points  for  the 
si x-degree-of- freedom  body  motion  based  on  the  velocity  commands  from 
the  Man-Machine  Interface  block  according  to  the  automatic  body 
regulation  plan  In  coorperatlon  with  the  terrain  information  from  the 
Terrain  Estimation  block.  All  the  six  velocity  components  are  filtered 
to  get  the  realizable  values  to  the  vehicle  system.  The  body  velocity 
set-points  are  passed  to  the  Leg  Coordination  block  In  order  to  compute 
the  optimal  period  which  Is  used  to  determine  whether  the  vehicle  needs 
to  slow  down.  If  It  is  necessary  to  slow  down  the  vehicle  speed,  each 
of  the  three  velocity  components:  vx,  vy  and  wz,  Is  reduced  by  the 
amount  of  the  slowdown  factor  which  Is  passed  from  the  Leg  Coordination 
block. 

The  body  position  Is  updated  by  the  three  body  translational 
velocity  components,  and  the  body  orientation  is  defined  by  the  three 
body  rotational  velocity  components.  The  updated  Information  of  the 
body  position  and  orientation  Is  stored  in  the  homogeneous  transforma¬ 
tion  matrix,  H.  Figure  6.4  shows  the  flow  chart  for  this  block. 

6.2.3  Leg  Coordination 

The  change  of  the  size  of  the  constrained  working  volume  Is  also 
made  In  this  block  according  to  the  predicted  foot  velocity  of  the  leg 
while  In  the  transfer  phase.  Then,  the  optimal  period  is  computed  using 


the  new  constrained  working  volune.  If  the  optimal  period  obtained  is 
too  short  to  properly  coordinate  the  leg  stepping  sequence,  the 
deceleration  plan  Is  Invoked  to  compute  the  slowdown  factor  which  Is 
passed  to  the  Body  Set-Point  Generation  block.  Also,  the  transfer  time 
Is  set  to  the  transfer  time  threshold  and  the  new  period  for  slowdown 
is  computed. 

This  block  updates  the  kinematic  cycle  phase  variable,  t,  for  each 
execution  cycle  of  the  program  according  to  the  period,  t,  to  determine 
the  periodic  leg  stepping  sequence.  By  using  the  updated  value  of  4, 
the  leg  phase  variable,  $  ,  of  leg  1  Is  defined.  Then  the  leg  phase 

variables  are  passed  to  the  Foot  Trajectory  Generation  block. 

Figure  6.5  shows  the  flow  chart  for  this  block. 

6.2.4  Foot  Trajectory  Planning 

The  leg  phase  variable  passed  from  the  Leg  Coordination  block 
specifies  the  state  of  leg  1:  support  phase  or  transfer  phase.  When 
leg  1  Is  In  the  support  phase,  the  support  trajectory  Is  generated  to 
move  the  vehicle  according  to  th.e  body  velocity  and  position  defined  In 
the  Body  Set-Point  Generation  block.  When  leg  1  Is  In  the  transfer 
phase,  the  transfer  phase  variable,  ,  Is  computed  to  determine  which 
subphase  leg  1  belongs  to,  and  to  generate  the  appropriate  foot 
trajectory  for  the  corresponding  subphase.  Figure  6.6  shows  the  flow 
chart  for  this  block. 

6.2.5  Terrain  Estimation 

This  block  estimates  the  local  terrain  surface  as  a  2-D  plane  using 
six  points  of  estimation  passed  from  the  Foot  Trajectory  Planning  block 


through  linear  regression.  The  three  coefficients  o*  the  plane  equation 
are  defined  as  the  components  of  Ezj  of  the  terrain  coordinate  system. 
Then  the  homogeneous  transformation  matrices,  T  and  G,  are  computed. 
Figure  6.7  shows  the  flow  chart  for  this  block. 

6.2.6  Vehicle/Terrain  Simulation 

There  are  two  Independent  modules  developed  for  Interactive 
generation  of  the  prismatic  terrain  models.  As  shown  In  Figure  6.8,  In 
the  terrain  profile  generation  routine  (TERPRF),  the  terrain  profile  is 
interactively  generated  on  the  display  screen  using  the  joystick.  Then 
the  terrain  profile  (TR.DAT),  which  is  defined  In  the  Xg-Zg  plane.  Is 
passed  to  the  terrain  display  routine  (TERDIS).  The  display  routine  Is 
responsible  for  generating  the  3-D  terrain  database  defined  as  "Planes" 
and  a  homogeneous  viewing  transformation  matrix  defined  as  "EyeSpace" 
for  the  perspective  view  of  the  prismatic  terrain  by  using  the  joystick. 
The  database  for  the  prismatic  terrain  (PLANES.DAT)  and  that  for  the 
viewing  angle  (EYESPACE.DAT)  are  passed  to  the  control  program. 

The  vehicle  Is  drawn  as  a  stick  figure.  Upon  receiving  the  desired 
body  and  foot  positions  from  the  motion  planning  block,  the  perspective 
view  of  the  ASY  Is  displayed  on  the  prismatic  terrain  according  to 
the  viewing  transformation  matrix.  In  addition,  software  Implementation 
for  contact  and  proximity  sensors  Is  also  Included  in  the  Vehicle 
Simulation  block  using  the  prismatic  terrain  database. 

The  object  description  data  files  for  the  display  of  the  terrain  and 
vehicle  models  are  defined  in  the  format  described  In  [43].  In  this 
format,  a  solid  object  Is  defined  as  a  set  of  adjoining  polygons.  Since 
neighboring  polygons  share  vertices  along  common  edges,  objects  can  be 
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Figure  6.7.  Flow  Chart 
for  TEREST, 


Type  OnePlane  *  record 

UnltNormal:  Vector; 
01st:  real; 

Xmln,  Xmax:  real; 
end; 

var  Planes:  array£l.«50]  of  OnePlane; 
EyeSpace:  array£l..4,1..4]  of  real; 


Figure  6.8.  Oatabase  for  Terrain  and 
Viewing  Angle. 


more  compactly  defined  by  first  listing  all  the  vertices  belonging  to 
the  object  and  then  listing  polygons  by  the  numbers  of  the  vertices  they 
use. 

6.3  Evaluation  of  Control  Algorithms 

The  control  algorithms  for  locomotion  over  uneven  terrain  developed 
in  this  simulation  work  are  composed  of  three  different  algorithms: 

1)  Algorithm  for  body  altitude  control, 

2)  Algorithm  for  the  position  change  of  the  constrained  working 
volume  of  the  middle  leg, 

3)  Algorithm  for  the  change  of  the  size  of  the  constrained  working 
vol ume. 

As  discussed  In  Chapter  5,  each  algorithm  provides  more  stability  for 
the  vehicle  motion  than  that  for  the  vehicle  motion  without  It.  Thus  It 
may  Increase  adaptability  of  the  vehicle  to  terrain  Irregularities. 

To  evaluate  the  performance  of  each  algorithm.  It  was  desired  to 
decide  the  type  of  the  terrain  model  and  the  vehicle  motion  for  purposes 
of  comparison.  Since  the  motion  planning  of  the  vehicle  Is  based  on  the 
estimated  plane  of  support  which  may  be  regarded  as  a  simple  slope.  It 
was  reasonable  to  choose  a  sloped  terrain  model.  Also,  for  simplicity, 
the  vehicle  motion  was  limited  to  the  forward  motion  only  with  the  crab 
angle  zero,  and  the  body  was  kept  parallel  to  the  slope. 

In  this  test,  thirty  different  slopes  from  0  to  30  degrees  were  used 
for  the  terrain  models.  The  length  of  each  slope  In  the  direction  of 
the  forward  motion  was  40  ft  and  the  stability  margin  of  the  vehicle 
motion  over  each  slope  was  chosen  as  the  smallest  one  while  the  vehicle 
was  moving  10  ft  long  up  the  slope. 


Each  algorithm  was  tested  Independently.  When  one  algorithm  was 
tested,  the  other  two  were  not  Implemented  In  the  test  program.  The 
position  of  the  constrained  working  volume  was  defined  In  the  same 
way  as  shown  In  Figure  4.2,  unless  otherwise  stated. 

For  the  evaluation  of  the  first  algorithm,  the  positions  and 
dimensions  of  the  constrained  working  volumes  of  the  six  legs  were  fixed 
such  as  dxj  3  2.8  ft,  dyj  3  2.0  ft,  and  dzf  3  2.8  ft.  The  forward 
velocity  was  fixed  to  1.5  ft/ sec.  The  test  was  done  for  three  different 
values  of  the  body  height,  which  were  4.5  ft,  5.0  ft,  and  5.5  ft,  for 
each  of  three  different  duty  factors:  1/2,  2/3,  and  5/6.  Figure  6.9 
shows  the  results  of  this  test.  It  clearly  shows  that  the  stability 
margin  for  the  given  slope  angle  Increases  when  the  body  height 
decreases  for  each  duty  factor. 

The  test  of  the  second  algorithm  was  performed  under  same  conditions 
as  given  for  that  of  the  first  algorithm  except  that  the  body  height 
was  fixed  to  5.4  ft.  For  purpose  of  comparison,  the  stability  margins 
were  also  computed  for  the  case  that  no  position  changes  of  the 
constrained  working  volumes  of  the  middle  legs  were  made.  As  done  In 
the  test  of  the  first  algorithm,  the  test  results  were  obtained  for 
three  different  duty  factors.  Figure  6.10  shows  that  the  second 
algorithm  resulted  In  the  drastic  Increase  In  the  stability  margin  for 
the  given  slope  angle  compared  to  the  case  of  no  position  changes  made 
In  the  constrained  working  volumes  of  the  middle  legs. 

For  the  test  of  the  third  algorithm,  the  body  height  was  fixed 
to  5.4  ft  above  the  slope,  and  the  body  velocity  was  chosen  to  be 
0.5  ft/ sec.  The  x  and  y  dimensions  of  the  constrained  working  volumes 
of  the  six  legs  were  computed  from  Eqs.  (5.85)  and  (5.86),  where 


Body  Height 


Position  of  Constrained  Workln 


vxmax  3  vymax  *  2.0  ft/ sec,  v^,,  3  Vyml n  3  0*25  ft/ sec,  dxmax  3  3.0  ft, 
dymax  3  2.2  ft,  and  dXfflfn  3  dymln  3  °*25  ft*  The  resulting  dimensions 
were  dxj  *  0.65  ft  and  dy-f  s  0.53  ft.  The  stability  margins  obtained  by 
employing  the  third  algorithm  Is  compared  to  those  of  the  case  when  the 
dimensions  of  the  constrained  working  volumes  of  the  six  legs  were  fixed 
to  be  dxj  3  2.8  ft  and  dyj  3  2.0  ft.  Figure  6.11  shows  the  result  when 
the  duty  factor  Is  2/3.  The  result  Indicates  the  better  stability  of 
the  vehicle  motion  as  expected. 

From  the  results  obtained  by  the  above  three  tests.  It  Is  expected 
that  the  better  result  may  be  obtained.  If  all  the  algorithms  for 
stabllllty  enhancement  are  functioning.  Figure  6.12  shows  the  result 
obtained  when  the  overall  control  algorithms  developed  in  this  work  was 
applied  to  the  same  terrain  as  used  In  the  above  tests  with  the  constant 
forward  velocity  1.5  ft/sec,  and  Table  6.1  shows  the  actual  parameters 
used  in  the  control  algorithms.  Compared  to  the  results  of  the  above 
three  tests,  the  result  given  In  Figure  6.12  shows  the  considerable 
Increase  In  stability,  especially,  for  the  large  slope.  It  Is  also 
noted  that  the  stability  margin  Increases  very  significantly  when  $3l/2. 
This  may  Indicate  that  the  algorithms  for  the  change  of  dimensions  of 
the  constrained  working  volume  are  more  effective  for  the  small  values 
of  e  than  for  the  large  ones. 

6.4  Summary 

In  this  chapter,  a  brief  description  about  the  software  to  Implement 
the  control  algorithms  and  interactive  generation  of  the  terrain  models 
developed  In  this  simulation  work  was  presented.  Also,  evaluation  of 
the  performance  of  the  control  algorithms  was  discussed  through  software 
slmul atlon. 
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The  program  modules  were  defined  to  coincide  with  the  functional 
blocks.  However,  they  were  loosely  modularized  by  allowing  the  global 
parameters  to  pass  through  modules.  The  problems  of  parameter  passing 
between  modules  need  further  Investigation  In  order  to  Implement  the 
control  software  on  a  multicomputer  system. 

The  tests  of  the  performance  of  the  three  different  control 
algorithms  were  done  Independently  by  computing  the  stability  margin 
over  the  thirty  different  slopes.  The  results  show  the  significant 
Increase  In  stability  for  each  of  the  three  algorithms.  Furthermore, 
when  all  the  algorithms  were  functioning,  the  Increase  In  stability  was 
more  significant. 

The  test  results  given  In  Section  6.3  may  be  changed  by  the 
different  values  of  parameters:  the  dimensions  of  the  constrained 
working  volumes,  the  body  height,  the  body  velocity,  terrain  conditions 
etc.  However,  the  results  show  the  general  trend  of  the  performance  of 
the  control  algorithms  developed  In  this  work. 


Chapter  7 

SUMMARY  AND  CONCLUSIONS 


In  this  dissertation,  the  problem  of  omnidirectional  control  for 
locomotion  of  a  walking  vehicle  over  rough  terrain,  specifically, 
"Type-0"  terrain  [6],  has  been  Investigated  through  computer  simulation. 
As  a  result  of  this  research,  the  first  complete  control  algorithm  based 
on  the  vehicle  kinematics  for  rough-terrain  locomotion  has  been 
developed.  It  should  allow  the  vehicle  to  move  over  rough  terrain  In 
any  direction  with  a  high  degree  of  stability.  The  results  have  shown 
that  If  the  appropriate  control  algorithms  are  developed,  the  use  of 
periodic  gaits,  which  may  provide  a  simpler  means  of  Implementing  leg 
coordination  than  that  of  free  gaits  [7,28,29],  Is  possible  for 
rough- terrain  locomotion. 

7.1  Research  Contributions 

The  Implementation  of  periodic  gaits  for  omnidirectional  control  has 
required  that  the  simple  notion  of  stride  length  adopted  In  previous 
control  algorithms  [8]  be  abandoned.  Instead,  a  new  concept  has  been 
Introduced  in  order  to  select  the  locomotion  cycle  period  for  periodic 
gaits  and  Is  based  on  the  reachability  of  the  leg.  The  key  concept  Is 
the  notion  of  the  constrained  working  volume  for  a  leg  which  Is  defined 
as  a  rectangular  subset  of  the  overall  leg  working  volume.  The  merits 
of  the  use  of  the  constrained  working  volume  Include  these: 


1)  Eliminating  leg  interference  by  avoiding  overlapping  constrained 
working  volumes, 

2)  Increasing  the  stability  by  changing  the  position  and  dimensions 
of  the  constrained  working  volumes  over  rough  terrain. 

To  provide  omnidirectional  control,  the  optimal  cycle  period  is 
selected  in  such  a  way  that  at  least  one  leg  fully  utilizes  its 
constrained  working  volume.  Also,  each  foothold  is  selected  in  such  a 
way  that  the  foot  is  predicted  to  pass  through  the  center  line  of  the 
constrained  working  volume.  In  addition,  a  simple  deceleration  strategy 
for  the  body  velocity  has  been  developed  so  as  to  keep  each  leg  within 
the  physical  limits  of  Its  transfer  time.  While  omnidirectional  control 
was  not  possible  in  previous  work,  the  use  of  the  constrained  working 
volume  to  define  the  locomotion  cycle  period  and  leg  footholds  has 
resulted  in  this  new  capability. 

To  control  body  motion  over  rough  terrain,  the  local  terrain  surface 
is  estimated  by  using  six  points  of  estimation  based  on  the  measurements 
from  the  six  legs.  Since  the  six  estimation  points  may  Include  the 
potential  footholds  detected  through  use  of  the  proximity  sensors,  they 
may  allow  the  body  to  pitch  Into  a  depressed  surface  even  before  a  leg 
steps  into  that  area.  This  aids  In  the  leg  being  able  to  reach  that 
surface.  Also,  In  order  to  Increase  the  safety  and  adaptability  of  the 
vehicle  for  rough-terrain  locomotion,  a  simple  body  regulation  plan  has 
been  designed  which  results  In  the  body  attitude  adjusting  to  the 
terrain  slope  and  the  body  height  decreasing  with  greater  slope. 

To  control  the  leg  motion  In  the  transfer  phase,  a  simple 
rectangular  type  of  foot  trajectory  has  been  developed.  It  allows  the 
proximity  sensor  to  detect  the  potential  foothold.  Also,  the  proximity 
sensor  is  used  to  control  the  footlift  height  during  the  transfer 


forward  subphase  In  such  a  way  that  the  foot  trajectory  follows  terrain 
variations.  Therefore,  the  use  of  proximity  sensors  for  the  leg 
transfer  trajectory  has  been  completely  defined. 

7.2  Research  Extensions 

As  a  result  of  this  research,  it  is  clear  that  the  control 
algorithms  developed  need  to  be  Improved  to  some  extent  and  several 
problems  may  need  to  be  solved  in  order  to  actually  implement  the 
control  algorithms  on  the  actual  vehicle  (ASV-84).  The  following 
paragraphs  describe  the  future  work  proposed. 

One  of  the  major  aspects  of  the  algorithms  developed  in  this 
research  Is  that  they  make  use  of  periodic  gaits.  Since  the  optimal 
period  Is  determined  each  computation  cycle.  It  Is  constantly  changing 
during  locomotion.  By  freeing  the  cycle  period  so  that  It  may 
continually  change,  the  results  of  using  periodic  gaits  tend  to  give  the 
flexibility  previously  achieved  only  with  free  gaits.  However,  the 
algorithms  for  periodic  gaits  tend  to  be  computalonally  simpler  and  to 
date,  have  provided  the  only  basis  for  control  of  an  actual  vehicle.  An 
appropriate  direction  of  future  work  may  be  to  loosen  the  limitations 
Imposed  by  using  periodic  gaits  so  as  to  gain  the  advantages  of  free 
gaits  while  at  the  same  time  continuing  to  take  advantage  of  their  ease 
of  Implementation. 

The  body  regulation  plan  was  designed  In  a  simple  manner.  It  needs 
more  refinement  In  order  to  achieve  better  safety  and  stability  of  the 
vehicle.  Since  body  attitude  and  altitude  control  Is  limited  by  the 
leg  working  volumes.  It  Is  desirable  to  find  an  optimal  regulation  plan 
based  on  the  kinematic  limits  of  the  leg. 


The  use  of  proximity  sensors  for  the  body  and  leg  motion  planning 
has  been  Investigated  In  this  work  through  simulation  and  turns  out  to 
be  quite  useful  In  generating  body  and  leg  trajectories.  There  are  a 
number  of  difficult  problems  Involved  In  practically  Implementing 
proximity  sensors  and  these  Issues  need  to  be  addressed.  It  Is  clear  to 
the  author,  though,  that  the  adaptability  of  the  vehicle  to  rough 
terrain  will  Improve  considerably  by  using  proximity  sensors  In  future 
vehicles. 

From  the  test  results  given  In  Section  6.3,  It  may  be  noted  that  the 
stability  margin  Increases  as  the  duty  factor  Increases.  However,  a 
larger  value  of  the  leg  duty  factor  requires  a  shorter  transfer  time 
which  Is  mechanically  limited.  The  desired  result  Is  that  a  large  leg 
duty  factor  be  used  at  low  speeds  but  smaller  values  be  used  at  high 
speeds  so  as  that  these  speeds  may  be  achieved.  This  suggests  that  If 
an  automatic  “gait  shift"  algorithm  Is  Implemented  as  a  function  of  the 
vehicle  speed,  the  mobility  of  the  vehicle  mqy  be  Improved. 

The  deceleration  plan  developed  Is  based  on  the  simple  notion  of  the 
transfer  time  threshold.  To  apply  this  scheme  to  the  real  vehicle.  It 
should  be  modified  so  that  the  actual  limitations  In  the  acceleration 
and  deceleration  capability  of  the  leg  may  be  carefully  considered. 

One  of  the  major  problems  that  must  be  solved  Is  to  reduce  the 
computation  time  for  real-time  control.  Otherwise,  the  control  may  not 
be  stable  especially  at  high  speeds.  This  problem  may  be  solved  by 
Implementing  the  control  software  on  a  multicomputer  system  and 
Investigations  Into  this  area  should  proceed. 

An  Interesting  approach  to  the  development  of  the  control  algorithms 
for  locomotion  of  walking  vehicles  that  has  first  been  used  In  this 
work,  lies  In  the  application  of  the  Computer-Aided  Design  (CAD) 


techniques.  The  capability  of  the  CAD  technique  developed  In  this 
research,  though.  Is  limited  to  the  Interactive  change  of  two  control 
parameters--duty  factor  and  foot!  1ft  height.  Also,  the  capability  of 
Interactive  terrain  generation  Is  limited  in  generating  the  simple 
prismatic  terrain  models.  By  extending  the  present  CAD  technique  to  the 
extent  that  future  researchers  may  change  many  of  the -parameters  related 
to  the  control  algorithms  and  more  complex  terrain  models,  and 
Interactively  obtain  results  similar  to  those  given  In  Section  6.3,  the 
CAD  technique  will  provide  a  versatile  design  tool  In  developing  and 
evaluating  control  algorithms  through  computer  simulation. 

Future  work  was  proposed  In  the  proceeding  paragraphs  as  a  result  of 
this  dissertation  work.  Further  extensions  to  this  work  will  hopefully 
lead  to  more  sophisticated  and  efficient  control  algorithms  for 
rough- terrain  locomtlon  of  walking  vehicles. 
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is**********************************************************************! 

<  Pile  :  GLOBAL. PIS  > 

<  This  til*  is  for  the  global  typo  and  constant  declaration.  > 

t  > 

( *****************  **************************************** *************** 

typo  Vector  *  record 

x,T,Z  :  real; 
ondi 

Polygon  *  record 

Start# PolyVtces:  integer! 
ondt 

Plan*  •  record 

UnitNoraal:  VoetorJ  Oist  :  reolt  <  plan*  coofficionts  > 
ondt 

OnePlane*  record 

UnitNoraal!  VoetorJ <  plan*  coofficionts  of  torrain  > 

Oist  :  roalt 

lain#  XeaxJ  reeltf  boundaries  of  a  plana  in  X-direction  > 
ondt 

Matrix  ■  array  Cl. .43  of  VoetorJ 
Matrix*  «  array  C1..4#1..43  of  roalt 
Array6  *  array  C1..63  of  real t 
Tactile  ■  sat  of  1..6J 
Array3  ■  array  CO. .23  of  roalt 
String  «  packed  orrayC1..103  of  chart 
ArrayAVoctor  •  arrayC1..63  of  Vector: 

Array52Vector  •  arrayC1..323  of  Vector: 

ArrayMaxPtsVector  ■  arrayCl..S03  of  VoetorJ 
ArrayHaxPolysPolygon  *  arrayCl..S03  of  Polygon! 

ArrayNaxPelysOnoPlan*  ■  arrayCl..S03  of  OnaPlanot 
ArrayMaxVtcoslntoger  a  arrayC1..1003  of  integer: 

Array6Bool*an  a  arrayC1..63  of  boolean: 


const  MaxPtS  a  S0| 

MaxPolys  ■  SOI 
MaxVtcos  a  loot 
PI  a  3.1415927: 

PI2  •  1.5707963!  {  PI/2  > 

NooinalHoight  ■  S.4i  t  noainal  body  hoight  in  foot  ) 

Xhip  a  Array 6(  5.0,5.0#0.0«0.0#-5.0»-5.0  )J  €  position  of  hip  in  foot  > 
Thip  *  2.251  <  hip  joint  position  3 

<  dioonsion  of  rectangular  hexahedron  eorking  voluoe  > 

XL  a  3.0 J  VL  a  2.2!  <  for  original  UV  > 

Zoax  •  -4.0J  Zoin  *  -6.8!  <  vertical  kineeatic  Unit  in  body  > 

LiftPhase  >  0.2!  <  transfer  phase  value  to  begin  liftoff  > 

PlacePhese  ■  0.8!  <  transfer  phase  value  to  begin  placeoent  > 


t. 


t**a*ea*** ************ **e********************************e************e*» 
<  Pile*  MAI NVAR.PAS  } 
(  This  file  is  for  tho  variable  declaration  for  tSVNUN.  ) 
(  » 
{*************************************************«************* ********) 


VTMax  *  1.5; 


(  max  foot  velocity  in  transfer  > 


Caoitals  ■  *A*..*Z*i 
CharSet  *  set  of  Caoitals; 


Setalodot 
I*  J.  PilMua 

:  integer! 


Letter 

HaltSet 

Velocity 


EyeSpace 

H.  IHVHi 
Si  iNVGt 
T,  INVT 

Body  . 

Points 

Polygons 

: 

Planes 

o 

• 

Vortices 

! 

RefPost, 

PPOE. 

PPSEt 

PPB# 

•Rfl 


!char i 
:CharSeti 


4rray3 ! 


Vector! 


Matrix*: 


Matrix! 


f  leg  duty  factor  index 


t  loop  index 

<  sot  of  commands  for  ahich 

<  vehicle  halts  aftereards 


I  homogeneous  matrix  froa  object  to  eye  space 

<  transform  aatrix  froa  body  to  earth  > 

<  transfora  aatrix  froa  body  to  terrain  > 

<  transform  aatrix  froa  terrain  to  earth  > 


<  points  of  vehicle  in  body  coord  > 

!  ArraySZVector ! 

<  points  of  object  in  reference  fraaa  > 

!  Array Max Pts Vector! 

1 

:  ArrayMaxPolysPolygon! 

!  ArrayMaxPolysOnePlanei 

( 

!  ArrayMaxVtcesInteger ! 

1  <  roforence  position  of  CWV  > 

<  end  point  for  foot  trajectory  in  earth  ) 

<  six  estimating  points  for  support  plane  > 

<  present  foot  position  in  body  > 

<  present  foot  velocity  > 


!  Array6 Vector i 


:  Vector! 

Contact 

:  Tactile! 

NuaPolys 

:  integer! 

dx  t  dy « 

RPhaset 

LPhase 

!  ArrayBi 

Coaaand 

:  char! 
Vaax»  Vain( 
dxaax,  dxain« 
dyaaxt  dyain. 


<  body  translational  rate  > 

f  body  rotational  rate  > 

(  set  of  supporting  legs  1 

<  total  number  of  polygons  to  dram  > 

<  x  and  y  dlaensions  of  CUV  > 

<  relative  leg  phase  ) 

(  leg  phase  variable  » 

<  operator  input  command  ) 


t  P J  JPi? 


Seta* 

or, 

FootLif t, 

*V£LX,  NVELY*  NRVELZ. 
Period* 

Phase* 

etat 

:  real: 


:  Plan*; 

EstimateFlag 

1  boolean; 

Liftof fFlag* 

Tp*n*f erFlag* 

PlaceFlag 

t  Array 63ool«an : 


<  log  duty  factor 

<  delta  tie*  (  sec  > 

<  foot  lifting  height 

(  period  of  kinematic  cycle 

<  kinematic  cycle  phase 

C  angle  beteeen  gravity  and 

<  terrain  noreal  vector 

<  support  plan*  in  body  coord 

C  support  plan*  in  earth  coord 


<  flag  for  estimating  support  plan*  » 


<  flag  for  Liftoff  phase  > 

{  flag  for  Transf erf oreard  phase  > 
C  flag  for  Placement  phase  > 


P 

li> 


I 


ua 


\Ljd 


1 

% 


<******************************** ************************* **************) 
l  films  EXTERNAL. PAS  > 

(  This  fil*  is  for  ths  external  declaration.  > 

<  > 

<***********************«***********************************************> 

function  SignC  X  :  real  )  :  rsal. 

Extarnal • 

function  Atan2(V(  X  :  real)  :  raal; 

Extarnal  i 

function  ArcCosC  S:  raal):  raal: 

Extarnal: 

function  Tan(  S:  raal  )i  raal: 

Extarnal; 

function  OotProdC  Ptl*Pt2:  Vactor  ):  raal: 

Extarnal: 

procedure  VactSubC  Ptl*Pt2:  Vactor:  war  Pt3:  Vactor  >; 

Extarnal: 

procadura  CrossProdC  Ptl»Pt2:  Vactor:  war  UnitNoraal:  Vactor  )• 

Extarnal; 

procadura  IdentC  war  Mtx  i  Matrix*  )i  t  identity  Matrix  ) 

Extarnal; 

procadura  RotateMatC  Axis  :  char;  Co*A,SlnA:  raal:  war  Mtx  :  Matrix*  ); 
Extarnal; 

procadura  TransMatC  X.r.Z:  raal;  war  Mtx:  Matrix*  ): 

Extarnal; 

procadura  Vact*Transf or*C  Mt  :  Matrix*;  Pt  :  Vactor;  war  Na«Pt  :  Vactor  ); 
Extarnal; 

a 

procadura  VaetTransf or*<  Mt  i  Matrix:  Pt  :  Vactor;  war  N**Pt  :  Vactor  ); 
Extarnal; 

procadura  PlaneTransfor*<  P  :  Plana;  Mt  :  Matrix)  war  Na*P  :  Plana  )i 
Extarnal: 

procadura  Mat*Mult<  Mtl»Mt2  :  Matrix*;  war  Pasult  :  Matrix*  ): 

Extarnal: 

procadura  MatTransposeC  Mt  :  Matrix:  war  TMt  :  Matrix  ); 

Extarnal: 

procadura  MatMultC  Mtl.Mt2  :  Matrix:  war  Result  :  Matrix  ); 

Extarnal: 

procedure  Matlnwerset  Mt  :  Matrix;  war  XMt  :  Matrix  )! 

External: 

procadura  Orthogonaliiation<  war  Mt  :  Matrix  >; 

Extarnal: 
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i? 


>  ■>"->! 


:3 


procedure 


External; 


ReadQbjectC  ear  Filename  :  String; 

var  NuePolys  :  integer; 
var  Point*  :  Arr ayMaxPts Vector ; 
var  Polygons  :  ArrayMaxPolysPolygon; 
var  Vertices  s  ArrayMaxVtcesInteger 


procedure  MakeOisp layablei  var  Pt  :  Vector 
External; 


procedure  DisplayOb jec t(  Mt  :  Matrix*; 

OraeAgain  :  Boolean; 
var  Nuapolys  :  integer: 
var  Points  •  ArrayMaxPtsVector; 
var  Polygons:  ArrayMaxPolysPolygon; 
var  Vertices  :  Arr ayMaxV tees Integer 

)  I 

External : 


PROCSOURE  JOTSTICICC 
External : 


VAR 

): 


OATA 

SCALE 


ARRAT3; 

REAL 


procedure  WorkVoluaeC  var  Polygons  :  ArrayMaxPolysPolygon; 

var  Vertices  :  ArrayMaxVtcesInteger J 
var  Body  :  ArraySZVector 
>: 

External: 

procedure  vehicleC  var  FPEt 

FPOE  :  Vector; 

PrxSensort 

ConSensor  i  boolean; 
var  Contact  :  Tactile: 
var  h  :  Matrix: 
var  dxt 

dy  :  Array 6 ; 

var  RefPosB  :  Array6Vector ; 

var  FPB  :  Array6 Vector : 

var  SyeSpace  :  Matrix*; 

var  MuaPolys  :  integer; 

var  3ody  J  Arr ay S2 Vector ! 

var  Points  :  ArrayMaxPtsVector ; 

var  Polygons  :  ArrayMaxPolysPolygon: 

var  Planes  i  ArrayMaxPelysOnePlane! 

var  Vertices  :  ArrayMaxVtcesInteger 

)  • 

External: 

procedure  SupportPattsrnI  var  M  I  Matrix; 

var  Points  :  ArrayMaxPtsVector: 
var  Contact  :  Tactile 

>; 

External; 

procedure  EstiaatePlaneC  var  SupoertPlane  ;  Plane! 

var  PPSE  *  ArrayBVector 
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k 


%> 

22 


=> 

*/ 


5 


% 


r»\ 


External ; 


): 


I 


i 

.  .. 

,  * 
X.1 


procadura 


Extarnal ; 


TarrainCoordC  var  H, 

T, 

INVT, 

5  . 

INVG  :  Matrix: 
var  Etal  :  raal; 
var  SPE, 

SPB  :  Plana 


procadura  GatlntarsactionC  var  ClipXT. 

ClipXB, 

ClipYL  , 

ClipYR t 
XI. 

Y1  , 

X2. 

Y2  :  raal: 
var  Error  S  Intagar 

>r 

Extarnal; 


procadura  OptiiaalParicdC 


var  SodyTransRata, 

SodyRotataRata  :  Vactor: 
var  RafPoaS, 

FPS  :  «rray6Vactor : 


£ 


:v 

s 

i 

z? 

$ 


a 

c 

s? 

i 


u 

i 


var  dx. 
dy. 

LPhasa  :  *rray6; 
var  Contact  :  Tactila; 
var  Sata. 

Pariod  :  raal 

): 

Extarnal: 


procadura  LagCoordinatlonC 


var  M. 

INVM  :  Matrix: 
var  SP9. 

SPE  :  Plana; 
var  BodyTr anaRata. 

SodyRotataRata  :  Vactor; 
var  RafPoat. 

FPS  :  «rray6Vactor ; 
var  dx. 

•  dyt 
RPhaaa. 

LPhaao  s  Array6: 
var  Contact  :  Tactila! 
var  Viaax.  Vain, 
dxaax.  dxoln. 
dyaax.  dyaln. 

DT, 

Sata. 

PMaaa* 

"arlod. 

SloadoanF actor  :  raal 


v 
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0 


External 


>; 


procedure  BodyServoC  ear  M, 

INVH  :  Matrix: 
var  SMB  * 

SPE  :  Plana: 
var  BodyTransRate* 

BodyRotat eRate  :  Vector; 
var  RefPosB* 

PPB  :  ArrayBVector ; 
var  dx« 
dy* 

RPhasa* 

LPhase  S  Arre y6» 
var  Contact  :  Tactile: 
var  Vaax*  Vein* 
dxaax*  dxain* 
dyaax*  dyein* 

DT, 

Bata* 

Phase* 

Period* 

Etal* 

NVELX * 

NVELT, 

NRVELZ  :  real 


procedure  FootTraJeetoryC 


var  H* 

Invm  :  Matrix: 
var  BodyTransRate* 

■odyRotateRate  :  Vector: 
var  RefPosB* 

PPOE, 

FPSE, 

PPB* 

PRB  :  Array6Vector : 
var  dx« 

PV* 

LPhase  :  RrrayB; 
var  Contact  :  Tactile: 
var  PootLift*  DT* 

Seta* 

Period  :  real; 
var  CstiaateFlag  :  boolean: 
var  LiMoffPlag* 

TransferPlag* 

PlacePlag  :  ArrayABoelean: 
var  EyeSpace  :  Matrix*; 
var  NuaPolys  :  integer: 
var  Body  !  Array SIVec tor ; 
var  Points  :  ArrayMaxPtsVec tor ; 
var  Polygons  l  ArrayMaxPolysPolygoni 
var  Pianos  :  ArrayHaxPolysOnePlane: 
vsr  Vertices  :  ArrayMaxVtcealnteger 


External: 


t Pv>«4.  -Hi 


■M  J,  .V  Jr. U  .W  Ml 


procedure  ASVINITC 


war  M, 

IHVH  t  Matrix; 
war  dx« 

dy  :  krray 6; 


war  Contact  :  Tactile: 
war  dxein# 
dy ein# 

3T, 

Foottif t  :  raal: 
war  BetaMode  i  integer! 
war  >*fP«iSi 

woe. 

wse, 

Wl  :  Array6V*ctor : 
war  EyaSpaca  :  Matrix*; 
war  NuePolys  :  integer: 
war  Body  I  Array 52 Vector ; 
war  Points  :  ArrayMaxPtsVector : 
war  Polygons  !  ArroyMaxPolysPolygon! 
war  Pianos  :  ArrayMaxPolysOnaPlana; 
war  Vortices  i  ArrayMaxVtcoslntagsr 
)S 

External ! 

procedure  Malt<  war  NVELX# 

MVELV, 

NAVELZ  :  raal) 
war  BodyTransAate# 

BodyRotateAate  !  Vector: 
war  FAB  :  ArraydVeetor 

>: 

External: 


procedure  Initialize; 


war  N  :  Matrix: 
war  dx« 
dy# 

A Phase# 

(.Phase  i  Array* ; 
war  AafPosB# 

FPOE. 

FPB  T  Array* Vector; 
war  FootLift#  Phase# 

Period# 

Beta  :  real: 
war  Contact  t  Tactile: 
war  EstiaatePlag  :  boolean! 
war  (.if  toff  Flag# 

PlacpFlag  :  ArrayBBooloan: 
war  EyoSpace  :  Matrix*: 
war  NuoPolys  :  integer: 
war  Body  :  ArreySZVector! 
war  Points  :  ArrayMaxPtsVector; 
war  Polygons  :  ArrayMaxPolysPolygon; 
war  Pianos  s  ArrayMaxPolysOnePlan*! 
war  Vertices  l  ArreyMaxVtcesIntegor 


External; 


$ 

I 


w  - 


procedure  XEVIMT;  MonPaseal; 


K  jc^}DBS8BECiesss£  m m&  y&2$&XLt&i»iob  «as*sH  *<obm 


i 


procedure  KCTCK  ver  Coaoend  I  char  ); 
procedure  KTKIIT!  NonPeecel; 
procedure  XCXILLt  NonPeecel; 


INQUEt  NonPeecel; 


procedure  NaaPiK  ver  PilNua  ;  Integer  >; 


procedure  StpNea:  Nc 


procedure  *oveto<  ver  IX, I f  ;  Integer  );  NonPeecel; 


procedure  9raato<  ver  IX, XT  :  integer  );  I 
procedure  StoverC  ver  PllNu*  ;  Integer  ); 


ss 


f? 


■S 


cp************e**ee*eeee*e**e*e*e***ee**e*e**e*******e***e***ee****ee***) 

<  Piles  asvh axn. pa $  > 

<  This  fils  is  for  the  ssin  and  the  een-aachlne  interface.  > 

<  > 
(****d*p****************eee**eea******e****e*eeeee***ee**ee**ee*eeeee***> 

prograe  ASVNAIn; 

tinclude  Global; 
tinclude  external: 
tinclude  Mainvar: 


begin  (  sain  prograe  > 

(  oaraeet or  Initialization  > 

4SVINITC  HdNVH.da.dy  (Contact  .dxeln.dyeln. OT.FootLif t» 

BetaNode.RefPosB.PPOE .PPSE.PPB.SyeSpace.NuePolys, 
Body .Points .Polygons .Planes . Vertices  ) J 

HaltSet  s«  C'N'.'N'.'l'.'H'.'x'a; 

C  set  tereinal  characteristics  > 


C ?4h*  ); 

c 

set  orign  to  rela 

C741 '  ): 

c 

set  to  noscroll  > 

C2J'  ): 

c 

erase  screen  > 

citin'  ): 

c 

C2J'  ): 

c 

ci:ih'  ); 

i 

cursor  to  hoee  > 

(.display  coenand  set  > 

eriteln: 

eritelnt 

eritelnC  CHRC27),  'sAeeeee  C3NNAN0S  aeeee'); 
eriteln: 

eritelnC 'Enter  CHRC27).'*4X','  TO  INITIALIZE  THE  LEG  POSITIONS'): 

eritelnC'  ',  CHRC27).  '  TO  START  NOTION'): 

eritelnC*  ',  CHRC27), 'PSH'. '  TO  HALT  NOTION'): 

eritelnC'  ',  CMRC27),'*6N','  TO  NOOIPT  THE  PARANETERS'): 

eritelnC*  '.  CHRC27). 'A4X*. '  TO  EXIT  THE  PROGRAN  EXECUTION'): 

eriteln: 

eriteln: 

ariteln: 

eritelnC  CHRC27). '•««*•*  REAL  TINE  OPERATION  Sees*'): 
eriteC  CHRC27),  'Cl8:22r'>:  C  set  scrolling  region  > 


Coeeend  »«  'N';  (  enter  eodify  sequence  initially  > 

kevint: 

repeat  (  until  Coaeand  *  'x'  ) 


if  Coeaand  in  HaltSet 
then 

begin 

case  Ceeeand  of 


begin 

Initial! 


It 

If 

If 

I) 

I? 


‘j  V  \i  NT 


PPS.PaatLi ft .Phase .Par lad* Beta. 

Cantac t*EstiaatePlag.Lif taffPlag. 
PlaceFlag.EyeSpace .NuaPelys.Sady . 

Paints .Pelygens.Planes.Varticas  ) 5 

Halt  <  NVELX  .NVELY .NRVELZ.BedyTransRate. 
BadyRatateRata.PRB  >; 

and! 

*H*i  Halt  (  NVELX .NVELY. NRVELZ. BadyTransRate. 

BodyRot at »Rate,FR8  >; 

*N*.  oagin 

•ritalnl 

•ritalnC  'DOES  BETAMOOE  >  BataNadall, 

'  NEEO  TO  BE  CHAN6E0?*); 
sriteC *  TYPE  A  SPACE  BEPORE  CY/N3**): 
tCKILL S  t  eaneal  input  raguast  > 
raadlnC  Letter  >t 

if  Lattar  >  *Y*  than 
rapaat 

•ritalnC 'PLEASE  ENTER  0ES1RE0  BETANOOEt*); 
•ritalnC*  It  BETA  ■  S/6  21  BETA  •  3/6*>: 

•ritalnC*  3t  3ETA  •  2/3  6i  BETA  •  1/2*): 

•ritalnC*  Si  BETA  »  11/12  '): 
raadlnCBatahada) t 
until  BataMada  in  C1.2.3.6.S3: 

easa  BataMada  af 

It  bagin  C  Bata  ■  S/6  > 

Vaax  i*  1.2S:  dxaax  S *  3.0t  dyaai  :■  2.2: 
Vain  t«  0.251  dnin  :■  0.T5;  dyain  t«  O.TSt 
andt 

2t  bagin  C  Bata  •  3/6  > 

Vaa*  !«  l.St  dxaax  :■  3.0;  dyaax  :•  2.2: 

Vain  :•  0.251  daain  t«  0.S:  dyain  t*  0.51 
and; 

3:  bagin  C  Bata  •  2/3  > 

Vnax  :•  2.0t  dxaax  1*  3.01  dyaaa  :•  2.2: 

Vain  :•  0.2S1  dxain  1*  0.251  dyain  t«  0.251 
•nd; 

6t  begin  (  Bata  •  1/2  ) 

Vaax  is  3.0:  dxaax  :•  3.0:  dyaax  :■  2.2: 

Vain  :•  0.251  dxain  1*  0.251  dyain  t»  0.29: 
•ndl 

andt  C  case  and  > 


case  BataMada  af 


It 

Bata 

:* 

0*8333: 

C 

S/6 

> 

2 1 

Bata 

ts 

0.7S1 

< 

3/6 

) 

3t 

Bats 

is 

0.666T; 

c 

2/3 

> 

6 1 

Bata 

t« 

o.s: 

c 

1/2 

> 

St 

Bata 

ts 

0.916 

(11/12) 

andtC  easa  ) 

C  assign  rslatiwa  lag  phasa  > 

RPhaseCIS  :>  0.0:  RPhas*C23  :•  0.5: 

RPhasaC33  t*  Bata:  RPhasaCA)  t«  Bata  -  0.51 


RPhaseCS)  :•  2  *  Bata  -  i.q;  RPhas*C63:*  RPhesoCS3*O.S ) 
RPhas*C63  :■  RPhas*C63  -  trunc(RPhas*C63): 


arita<  'Chang*  FootLif tCT/N3?  *  >t 
readlnC  Letter  ); 

If  Latter  •  *V*  THIN 

repeat 

oritolnC  'Enter  nee  value  of  PootLift'): 
readlnC  FootLlft  >1 

until  (FootLlft  >■  1.0)  and  (FootLift  < *  3.0): 


INQUE:  <  REASSERT  INPUT  REQUEST  > 

•riteln: 

oritelnC  CHRC2T),  'S6INITISLIZE  HEXAPQ3  *): 
end: 

end:  {  case  Coeeand  > 

ktuait; 

RE VCR (Coeeand)} 
end  (  then  ) 


ala* 

begin 

Joystick!  Velocity,  VT«ai)i 

NVELX  :«  Velocity C 13 : 

NVSLf  :«  -Velocity CO) ; 

NRVELZ  :«  Veloci tyC23  *  0.1} 

if  EstioateFlag 
then  begin 

EstleatePlanoC  SPE.FPSE  )} 

EatleateFlag  :■  falsot 
end: 

TerrainCoord(  rt,T, XNVT,C,INVG.Etal(SPE,SPB  >} 

SodyServoC  H, INVH, SPS • SPE, Body TranaRate, Body Rota teRate, 

Ref Po «B ,FPB, da , dy,RPhaa*,LPha*e, Contact, 

0T, Beta, Phaae, Period, Etal, NVELX, NVeLT, NRVELZ  )| 

Foot Tr ajoc tory(  H, INVH, BodyTran«Rate,BodyRo tat eRate, 

Ref Po*B,FPOE,FPSE,FPB,FRB,dx,dy , 
LPha**,Contact , FootLif t,0T, Beta .Period, 

!s tlaatePlag, Liftoff FI ag,Tran*ferP lag, 
Plac*Flag,EyeSpace,NuePoly«,Body, 

Points, Polygona, Planes, Vertices  )I 


KETCXC  Coeeand  ): 
endl  (  els*  ) 

until  Coeeand  »  'X'l 

ICRILL1 

arito  C  CHR(27>,  'C761')} 
erit*  <  CHRC27),  *C?*h'): 
arito  (  CNRC27),  *Cl}22r*)} 
erit*  (  CHR( 27 ),  'C2J')} 
erit*  (  CHRC27),  'ClllH')} 


<  sot  origin  to  absolute  nod*  > 
(  set  to  seooth  scroll  ) 

<  restore  scrolling  region  ) 

(  eras*  screen  > 

<  cursor  to  hoe*  > 


i.t, 


L*  ^  tJ>  Ul  Jti*. 


(Snoaain) 

(<oan> 

tOOdOdOO*aooeooooooooooo*OOOOOOoeoeoo*****eoooeoaOOOoooOdOOdooeooaooaood] 

<  Pila*  LI8R.PAS 

(  This  fils  is  library  far  ASVMAXN. 

< 

teooooeoooe*oooooeooeoooo*oeooooeoooeoooooeooa*o*aooooooeeeooeooeoe*eooo 


Tlnclude  global; 
lincludo  Mttrnali 


function  Sign(  X  :  real  )  I  real; 
begin 

if  X  »  0.0 

than  Sign  :•  1.0 
alts  Sign  :«  -1.0 
and;  (  Sign  > 


§ 


function  Atan2CTt  X  :  rail)  1  real; 
begin 

if  abs(X)  >  0.00001 

than  <  X  is  nonzero  > 
if  X  >  0.0 
than 

Atan2  :■  ArcTan<V/x> 

also 

Atan2  :•  ARCTanCY/X)  ♦  PI  e  SignC  T  ) 
<  and  if  X  ) 

else 

At an 2  :«  SI  /  2.0  a  SignC  T  )} 
and;  <  Atan2  I 


* 

8 

I 

I 


m 


function  ArcCosC  S:  real):  real: 

war  C  :  real; 

begin 

C  :•  sqrtCl.O  -  SOS); 

ArcCos  :«  Atan2(  C»S>; 
and;  <  ArcCos  ) 


function  TanC  S:  real  )t  real; 
begin 

Tan  :«  SinC  S  )/  CosC  S  )l 
and; 


function  DotProdC  MliPt2>  Vector  >:  real; 
begin 

aith  Ptl  do 

OotProd  *•  XOPt2. X  ♦  YdPtZ.Y  ♦  20Pt2.21 
and;  <  OotProd  ) 


procedure  VectSubC  Ptl,Pt2:  Vector;  war  Pt3t  Vector  ); 
(  wactor  sbtraction  of  Pt2  fro»  Ptl  > 


k 

| 

»*• 

16 


K 

!; 

t 


bagin 


•  ith  Pt3  do 
bagin 

x  :■  Ptl.x  -  Pt2.xi 
T  :»  Ptl.v  -  Pt2.Vl 
I  s«  Ptl.Z  -  Pt2.Zi 
and! 

•no;  <  VactSub  > 


procedure  CrossProdC  Ptl,Pt2l  Vactorl  war  UnitNoroal:  Vac to r  )! 
<  ganarata  unitnoraal  wactor  by  (Ptl  X  Pt2)  ) 

war  *  :  raal: 

bagin 

with  UnitNorwal  do 
bagin 

X  :•  Ptl.Y*Pt2.Z  -  Ptl.Z*Pt2.Vi 
V  !«  Ptl.Z0Pt2.X  -  Ptl.X0Pt2.Zi 
Z  :«  Ptl.X0Pt2.V  -  Ptl.Y0Pt2.Xi 
M  :■  sqrtC  XOX  ♦  YOY  a  20Z  )| 

x  :•  x/»j  y  :■  r/x:  z  s«  i/* i 
aodi 

ondi  t  CroasProd  > 


procadura  IdantC  war  rtx  i  Nitri>4  )i  <  idantity  Matrix  > 

war  IiJ  :  intagari 

bagin 

for  I  i*  l  to  *  do 

for  J  :•  i  to  4  do 

if  I  ■  J  than  NtxC X  •  J3  !•  1.0 
alto  *txCX«J3  :•  O.Oi 

and!  <  Idant  > 


procadura  RotataMatC  Pais  :  chart  CosAtSinAi  raall  war  Mta  :  Matrix*  )! 
bagin  (  gat  rotation  Matrix  of  a  glwan  axis  > 

Idsnt(Htx) J 
casa  Axis  of 


*X*  J 

bagin 

MtxC2t23 

:• 

CosAl 

NtxC2.33 

la 

-SinAl 

MtxC3.23 

:« 

SinAl 

NtxC3» 33 

la 

CosAl 

•v*  s 

andl 

bagin 

*txCl,13 

s« 

CasA  l 

NtxCl* 33 

la 

SinAl 

NtxC3 t 13 

:  ■ 

-SinAl 

NtxC3«33 

!« 

'  CosAl 

*1*  : 

and! 

bagin 

N«xCl(13 

I  ■ 

CosAl 

KtxC1.23 

la 

-SinAl 

MtxC2* 13 

1  ■ 

SinAl 

MtxC2*23 

la 

CosAl 

and} 

<  Rot) 

and! 

(  casa 
atahat  » 

and  > 

procadura  Translate  X»Y» Zi  raall  war  Ntxi  Matrix*  )l 
bagin  <  gat  translation  Matrix  > 

XdsntX  htx  )1 

MtxClf *3  !•  XI  MtxC2t*3  J«  Yi  NtxC3«*3  !■  Zt 
andl  (  TransMat  > 


pracadura  Vact4Transfara(  Nt  S  Matrix!;  Pt  :  Vactor:  war  NaaPt  :  Vactar  )> 

<  NaaPt  »  Mt  a  Pt  > 

bagin 

NaaPt. X  :•  Pt.xantCl.13  ♦  Pt.taMtCl.23  ♦  Pt.zantCl.33  ♦  NtC1.43: 

NaaPt. t  :»  Pt.xaMtC2.13  ♦  Pt.taMtC2.23  ♦  Pt.2antC2.33  ♦  NtC2»43: 

NaaPt. Z  :•  Pt.xaMtC3»13  ♦  Pt.tPNtC3.23  ♦  Pt.ZPNtC3.33  ♦  MtC3.433 

and*  <  Vactar4Tran«fara  ) 


pracadura  VaetTranataraC  Nt  :  Matrix:  Pt  :  factor:  war  NaaPt  :  Vactar  >1 

<  NaaPt  •  Mt  a  Pt  > 

bagln 

NaaPt. X  :«  Pt. XPMtCl 3.X  ♦  Pt.tPMtC23.X  ♦  Pt.ZPMtC33.X  ♦  NtC43.Xi 

NaaPt. t  J«  Pt.XPNtC13.t  ♦  Pt.tPMtC23.f  ♦  Pt.ZPNtC33.t  ♦  NtC43.t: 

NaaPt. Z  :•  Pt. XPMtCl 3. Z  ♦  Pt.tPNtC23.Z  ♦  Pt.2PNtC33.Z  ♦  MtC43.Zt 

and.  (  VactTransf ara  > 


pracadura  PlanaTranaferaC  P  :  Plana:  Mt  :  Matrix:  war  NaaP  :  Plana  ): 

(  NaaP  >  P  a  Mt  > 

war  4.  B.  C.  0.  M  :  raal: 

bagin 

4  :■  P. Uni tNaraal. XPMtCl 3.X  ♦  P.UnitNoraal.taMtClS.t 
♦P.UnltNaraal.ZPMtC13.Z  2 

3  :»  P. Uni tNaraal. XPMtC23. X  ♦  P.UnltNaraal .tPMtC23.t 
♦P.UnitNaraal.ZPMtC23.Z  I 

C  *a  P.UnltNaraal. XPNtC 3 3.X  ♦  P.UnitNaraal.taMtC33.t 
+P. Un i tNaraal. 2PNt C 33. Z  t 

3  S«  P.UnltNaraal. XPMtC43.X  ♦  •.UnltNaraal.tPMtC43.t 
*P. UnltNoraal.ZPMtC43.Z  ♦  P.Oiatt 
m  :«  aqrtC  4P4  *  BaB  a  CPC  ); 
aith  NaaP  da 
bagin 

aith  UnitNaraal  da 

bagin  X  :«  4/m:  t  :•  B/m;  z  :■  C/m;  and: 
oiat  :»  o/n: 
and: 

and:  <  PlanaTranafara  ) 


pracadura  Mat4Mult<  Mtl.Nt2  >  Matrix4l  war  Raault  :  Matrix4  >1 

war  I.J.K  :  intagarl 

bagin 

far  t  !•  1  ta  4  d.  C  Mtl  X  Nt2  > 

far  J  :■  1  ta  4  da 
bagin 

ftaaultCX.Jl  :*  0.01 
far  K  :■  1  ta  4  da 

taaultCX . J3  :•  RaaultCX. J3  ♦  MtlCX.R3PMt2CK.J3: 
andt 

and!  (  Nat4Mult  > 


pracadura  MatTranapoaaC  Mt  :  Matrix:  war  TMt  :  Matrix  ): 
bagin 


aith  TMtCl 3  da 


STO* 


bagin 

x  :«  Mtcu.x:  y  :»  ntC23.x:  z  :■  ntC33.x: 

Mdt 

•  ith  TMtC23  do 
bagin 

x  :«  Mtci3.v:  r  :■  MtC23.v:  z  MtC33.y; 

and: 

■  ith  TMtC3 3  do 
bagin 

x  s»  Mtcxo.z:  r  :■  «tC23.z;  z  :»  *tC3-3.z: 

andi 

■ith  TMtC*3  do 
bagin 

x  :»  Mtco.x:  y  :■  ntc*3.y:  z  :>  Mtc*3.z; 

and: 

<  MatTranaposa  ) 


$ 


procadura  MatMultC  Mtl»MtZ  i  Matrix;  war  Raault  :  Matrix  >• 
war  TMt  :  Matrix: 

I  :  intagar: 

oagin 

MatTranaposaC  Mtl#TMt  3: 
for  I  :■  1  to  4  do 

■ith  RasultCIX  do 
bagin 

X  :>  OotProdC  TatCl 3(Nt2CI3  >; 

V  :»  OotProdC  TatC23 • Mt 2CI 3  >: 

Z  :•  OotProdC  TMtC33.Mt2CI3  >t 
if  I  ■  * 

than  bagin 

X  i«  X  ♦  TMtCia.X; 
y  :•  y  ♦  Tntcxa.v: 
z  :»  z  ♦  Tatcxa.z: 
and; 

and: 

and:  <  MatMult  > 


■K 


procadura  MatZnwaraaC  Mt  :  Matrix:  war  XMt  :  Matrix  )t 
bagin 

MatTranapoaaC  Mt»IMt  3; 

■ith  IMtC*3  do 
bogin 

X  !•  -DotProdC  MtC43.MtCl3  >; 
y  -OotProdC  MtC*3,MtC23  3: 

Z  :*  -OotProdC  MtC43.MtC33  ): 
and: 

and:  <  Matlnwarao  > 


procadura  Or thogonalizationC  war  Mt  :  Matrix  )l 
<  uaa  Sraa-Sehiait  orthojonalization  procaaa  > 
war  it  N  :  raal; 
bagin 

■ith  MtCi3  do  (  firat  orthogonal  wactor  > 

bagin 

M  :•  agrtC  xox  a  y*y  a  z*z  >: 
x  :•  x/m:  v  :■  v/m:  z  :•  z/m: 

and: 


V-vT-f.T'." 


I  CM 


I 


■«**,*■*  • 


a  :«  OotPr od(  OtC13,*tC23  >t 

tith  MtC23  do  (  socond  orthogonal  voctor  > 

bogin 

x  :■  x  -  aoxtcis.x: 

T  S»  T  -  AO*t£13.TS 
2  :■  2  -  AOHtC13.2t 
N  !■  sqrt(  X*X  ♦  TOT  ♦  202  >S 
X  S*  X/m  T  :>  T/qj  2  :•  2/HJ 
•nd! 

CrossProdC  NtC13.MtC23.MtC33  )S  (  third  orthogonal  voctor  ) 


I  '«or 

m 


!S 


VQ 


(Snoaain) 

<foen) 

<*****************»*******************•******************************•**> 

<  Pile*  01  PLAY .PAS  > 

(  This  fils  is  for  graphics  display  routines.  > 

<  ) 
<***************** e**e**********«**********»ee***»********e**ee* ********) 

linclude  Global! 

Tinclude  External: 

procedure  ReadObjectC  war  Fllenaae  :  String; 

var  NuaPolys  !  integer; 
var  Points  :  ArrayMaxPtsVec tor : 
var  Polygons  :  ArrayMaxPolysPolygon; 
var  Vertices  :  ArrayMax Vtceslnteger 
>: 

var  If  J  •  NuaVtceSf  NuaPts  :  integer; 

F  :  text: 

begin 

resetCFf  Fllenaae#  *.C*T'):  t  open  input  file  > 

readlnCF# NuaPts#  NuaPolys);  <  read  in  points  > 
for  I  :«  1  to  NumPts  do 

aith  Pointed!  do 

readlnCF t  J,  X.  T.  Z); 

NuaVtces  S»  0>  <  Initialize  size  of  vertex  array  > 

for  I  :■  1  to  NuaPolys  do  <  read  in  polygon  descriptions  > 

aith  PolygonsCIl  do 
begin 

start  :■  NuaVtces;  <  start  point  in  vortex  array  1 

re  ad (F (  PolyVtces  )«  f  nuaber  of  vertices  > 
for  J  :•  1  to  PolyVtces  do  f  read  vertex  pointers  > 

readCFf  Vertices!  NuaVtces  ♦  J  1): 
readln(F):  <  go  to  next  lino  of  input  > 

NuaVtces  :«  NuaVtces  ♦  PolyVtces; 
end: 

Close(F);  <  close  the  file  > 

end:  <  ReadObJect  ) 


procedure  MakeOisplayableC  var  Pt  !  Vector  ){ 

const  Middle  «  VectorC  500.0,500.0,0.0  >; 
Scale  •  *000.0: 

begin  (  perspective  projection  ) 

Pt.x  :«  Scale  *  Pt.x/Pt.Z  ♦  Middle. x: 

Pt. V  :«  Scale  *  Pt.V/Pt.Z  ♦  Middle. V; 
end:  {  NakeOisplayable  ) 


procedure  0isplay0bject<  Mt  :  Matrix*; 

OraaAgain:  Boolean: 
var  Nuapolys:  integer; 
var  Points  I  ArreyMexPts Vector : 
var  Polygons:  ArrayNaxPolysPolygon; 
var  Vertices  :  Arr ayMaxVtcesInteger 
>; 


TaoPt  S  factor; 

IiJiHi 

IX, IV  :  intagar; 

OisplayPts 

!  arrayC1..103  of  racord  X,v:  intagar;  and. 


■  1  to  NuaPolys  do 
aith  PolygonsCIl  do 
bagin 

VactATranafaraC  *t,  PointsCVartieasCStsrt+PolyVtcas33*TapPt); 
MakaOisplayablaC  TapPt  ); 
aith  TapPt  do 
bagin 

IX  !»  truncCX)  ;  IT  :■  truncCY):  Mo vatoC IX , I Y ) : 
and, 

for  J  J«  1  to  PolyVtcas  do 
bagin 

VactATransforaC  Mt,Point*CVarticssCStart*J33,TapPt  ); 
MakaOisplayablaC  TapPt  >i 
aith  TapPt  do 
bagin 

IX  :>  truncCX):  XV  :■  truncCY);  DraatoCIX, IY) 
and: 

if  OraaAgain 

than  aith  0isplayPtaCJ3  do 

bagin  X  Ja  ix»  V  :•  IY!  and! 

and; 

if  OraaAgain 
than 
bagin 

aith  0isplay»tsCPolyVtcas3  do 
MovatoCX,V>; 

for  J  :■  1  to  PolyVtcoa  do 

aith  DisplayPtsCJ3  do 
OraatoCX, V) ; 

and; 

and; 

C  OisplayObJac t  ) 


&BVHS '-■> ^  ’_*■  A  VM  tWAftlMWBmWWWV  wmwTOj 


VAR 


AOSR 

ADBUF 


ORIGIN  1704008 
ORIGIN  1704028 


CHANNEL 

INTEGEROATA 


INTEGER:  <  */0  STATUS  REGISTER  > 

INTEGER!  <  A/0  SURFER  REGISTER  > 

INTEGER;  <  CHANNEL  INOEX  > 

INTEGER:  i  A/0  INTEGER  DATA  > 


Jv 

£ 


Cno«iin> 

($o«n> 

t********************* ************************** ************************> 
(  Fil*:  JOYSTICK. PAS  > 

{  This  is  for  th*  joystick  int*rf*c*  routine.  > 

<  > 

{*****************************************•*****************************> 

Tinclud*  Global; 
tinelud*  External; 

PR0CE0URE  JOYSTICK ( 


VAR  CATA 
SCALE 

): 


:  arrays; 
:  REAL 


$ 


LV 


3EGIN  <  JOYSTICK  ) 

FOR  CHANNEL  i>  0  TO  2  00 
BEGIN 

AO SR  :•  CHANNEL  *  400E:  (  SELECT  MULTIPLEXER  CHANNEL  > 

AOSR  :*  ADSR  4  l;  <  SET  CONVERSION  COMMAND  SIT  > 

WHILE  AOSR  ANO  2008  »  0  00  (  WAIT  FOR  CONVERSION  >; 

INTEGEROATA  :■  A08UF  ♦  1770008;  t  CONVERT  TO  2*S  COMPLEMENT  FORMAT  > 

OATAC  CHANNEL  1  S»  INTEGEROATA  /  10008  *  SCALE;  f  •SCALE  <«  DATA  <  SCALE  » 

eno; 

ENO;  (  JOYSTICK  > 
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ISnosain) 

(loan) 

<•*********»******************************************** ********ss*ooa**: 

(  Fils:  VEHICLE. PAS 

(  This  fils  is  for  drawing  ths  tSV-14,  sorting  woluaas>  and  support 

<  pattern  of  ths  wshicla  aotion. 

( 

(•••••»**s**sss*ss*s*s**ss***s***********s*s*s*******s***s*******ss**s** 

tincluds  Global; 

tincluds  Eatarnal; 

procsdurs  VshiclaC  war  PPE* 

PPOE  :  Vsc tor; 

PraSsnsor* 

ConSsnsor  :  boolsan; 
war  H  i  Hatriat 
war  da( 

dy  I  Array*; 

war  XafPosS  ?  Array6Vsctor J 
war  PP3  :  irray* Vector ; 
war  EysSsacs  :  Matrix*; 
war  NuaPolys  t  integer; 
war  3ody  t  Array52Vector : 
war  Points  :  ArrayHaxP tsVector l 
war  Polygons  t  ArrayMaaPolysPolygon; 
war  Pianos  i  ArrayMaaPolysQnePlane ; 
war  Vsrticss  :  ArreyMaaVtcesInteger 
)J 

war  PilNua  :  integer; 


procsdurs  ProaiaitySensorC  PPEs  Vector;  war  PPOE:  Voctor  ){ 
i  got  log  touchdoan  point  in  sarth  for  lag  in  placsasnt  phass  ) 

war  J  J  intsgsr; 

Jtsgin 

J  S«  l; 

<  find  plans  indsa  ohsrs  foot  is  on  and  rsturn  indsa  ) 
shils  note  PPE.X  <a  PlanesCJl.Xaaa  >  do 
J  :•  J  ♦  1: 

sith  PlanssCJl  do  (  got  touchdoan  point  in  sarth  coord  > 

aith  UnitNorasl  do 

FPOE.l  :■  -C  X4PPE.X  ♦  tafPE.T  ♦  Oist  >  /  z: 

aith  PPOE  do 
begin 

x  :«  ppe.x;  t  ;•  ppe.t; 
and; 

and.  (  ProaiaitySsnsor  > 


procsdurs  ContactSsnsort  war  PPCi  vector); 

<  PPS.Z  is  aodifisd  to  that  foot  tip  can  contact  ths  ground  ) 
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var 


begin 


•nd: 


I.J 

0 


:  integer: 
:  real: 


J  :»  1;  (  index  far  array  of  Planes  > 

<  find  plane  index  ehere  foot  is  on  and  return  index  > 
•hile  not(  FPE.X  <»  PlanesCJS. Xnax  )  do 
J  :»  J  ♦  l: 


eith  PlanesCJO  do  (  eoapute  distance  froa  foot  to  plane  > 

eith  UnitNoreel  do 

FP6.Z:*-<X**PS.XeT*FPe.Tr0iSt)/l: 


<  ContactSensor  ) 


procedure  Kineoatics: 

(  reverse  kineoatics  equation  of  ASV64  vehicle  > 

C  Z  axis  goes  upoard  in  body  and  earth  coordinate  > 

const  LI  «  0.8S  L2  •  4.0:  L3  •  1.4$: 

L4  •  0.$:  <  leg  deoensions  in  feet  > 

var  Cl,  SI. 

Cos*.  Sin*, 

0,  01.  02. 

Signl •  Sign2. 

Alpha.  Beta*  Genoa. 

Theta.  Tl.  T2. 

XPC,  TPC,  2PC 

:  reel: 

I.  J  :  integer; 


5 

I 

1  h 


k* 

V 


i 


begin  <  Kineoatics  ) 

for  I  :■  1  to  16  do-  <  transfern  points  of  body  > 

VectTransf ort(  H,3odyCZ3,PointsCZ3  >1 

for  I  :■  1  to  6  do  (  tranforo  points  of  legs  > 

begin 

if  I  in  Cl, 3, S3  then  Signl  :•  1.0  else  Signl  :«  -1.0: 
if  I  in  Cl, 4. 6]  then  Sign!  *•  -1.0  else  Sign2  :■  1.0; 

<  eoapute  abduction  angle,  translations  of  leg  > 

xpc  :•  FPBcn.x  -  xhipcn: 

TPC  :■  FPBCZ3.Y  -  Signl  *  Yhip; 

zpc  :•  FPscn.z: 

Tl  : ■  sqrtc  TPCsfPC  ♦  ZPCaZPC  -  L4*L4  >; 

(  abduction  or  adduction  angles  > 

Theta  :■  Atan2C  SignlSTPC.  -Signl*ZPC  )  -  Atan2(  L4.Signl*Tl  >: 
Cl  :■  CosC  Theta  >:  SI  :>  Sin(  Theta  >: 

case  Z  of 

1,2  :  32  :•  0.2  *  xpc: 

3,4, s,6  :  02  ;»  -0.2  *  xpc: 

end!  (  case  end  > 
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T2  :■  0.25  *  <  -YPC*S1  ♦  ZPC*Cl  ♦  L3  )J 
01  :>  Sign!  *  T2J 

Alpha  :■  Atan2C  02,-Slgn2*oi  >• 

0  :■  0.S  •  sqr  tC  01*01  ♦  02*02  >  /  Lli 
Bata  :■  ArcCasC  0  )i 

Gaaaa  !*  -Slgn2  *  CPI2  -  Alpha  -  Bata  )t  <  angla  betaean  sys2  l  4  ) 
CoaA  :•  Cast  Gaaaa  )>  Sin  A  :■  Sin<  Gaaaa  ); 

J  S-  CI-l)  *  3} 

aith  BadyC17*J3  da  <  updoan  sliding  jaint  pesltian  > 

bagin 

X  :>  XhipCIJ : 

V  :•  Slgn2*01*Sl  ♦  L3*S1  ♦  SlgnI*Vhip; 

2  :•  -Sign2*01*Cl  -  L3*Clt 
and! 

aith  BadyCIBaJI  da  (  knaa  pasitian  > 

bagin 

if  1  in  Cl. 23  than  X  :■  L2*CosA  ♦  XhlpCIl 
•Isa  X  :«-l2*CosA  ♦  xhipCXlt 

T  :■  -Sign2*L2*Sl*Sin A  ♦  Sign2«01*Sl  ♦  L3*S1  ♦  Slgnl*Yhlpt 

Z  :■  Slgn2*L2*Cl*SinA  -  Sign2*Dl*Cl  -  L3*Clt 

and] 

VaetTransfaraC  H,BodyC17»J3.PointsC17*J3  )i(transfara  in  basa  systeal 
VaetTransfaraC  M.BadyC 18*J J.PointsC 1B« J3  )l 
VaetTransfaraC  H,PPBCX3*PolntsC1903  >S 

and!  C  and  af  laap  far  I  > 

endl  (  Kinaaatlcs  > 


aracadura  OraaLagl 

<  draa  a  sagaant  from  his  Jaint  ta  fast  tip  > 

eanst  Middle  »  VactarC  300.0. BOO. 0.0.0  >1  • 
Scale  «  15.01 

var  I.  IX.  IT:  integer; 

Xaidth*  Taidth  :  real: 


procedure  OashLineC  xi.Yl.X2.TZ:  real: 

Horizontal  i  boolean  >; 

const  N  •  51 

var  I.  XXI.  IT1 «  1X2,  XY2 

:  integer: 

Xnt  J  real: 

bagin 

if  horizontal 

than  bagin 

XXI  :■  truncC  Scala  *  XI  ♦  Middle. X  X 
IY1  «■  truncC  Scala  *  Y1  ♦  Middle. T  >: 
and 

also  begin 

IT1  ta  truncC  Scala  •  Xl  ♦  Middle. Y  >l 


m  :»  truneC  Seal*  *  Tl  ♦  Middle. x  )s 
•  nd S 

<te«*t*(  XX 1  •  IT  1  ): 

tat  :•  (  xi  -  Xl  )  /  n; 
for  X  :•  I  to  H  do 
Bogin 

XI  :■  Xl  ♦  Intt 
if  Horizontal 

then  Boqin 

XX2  :•  trunc(  Sealo  *  Xl  ♦  Middle.*  >; 
if  odd<  X  >  then  OraotoC  1X2. IT1  ) 
ala*  MovotoC  1X2, ITi  >1 

•nd 

•lao  Bogin 

XT2  :•  truncC  Sealo  *  II  ♦  Niddlo.Y  >: 
if  oddC  I  )  thon  OraotoC  IX1.IT2  ) 

•lao  NovotoC  IX1.IT2  >: 

•nd ; 

end; 

•nd:  (  OashLino  > 


procedure  OraoKlioitC  Cantor:  Vectors  Xoidth, Yoidth  :  reel: 

OraoVoluoo  i  booloan  )S 


var  XB ,  XT,  VL.  TR 

:  real: 

1X8,  IXT ,  I VL,  XTR  :  integer: 

Bogin  <  OraoKlioit  > 

•ith  Cantor  do 
Bogin 

xs  :•  X  -  Xoidth:  VL  :•  V  -  Voidth: 

XT  :■  x  ♦  Xoidth:  tr  :«  V  ♦  Toldth; 
end; 

if  OraoVoluoo 

thon  Bogin 

OashLinoC  XB,TL,XT,TL,truo  ); 
OashLinoC  TL,XT,TR,XT,falso  )S 
OashLinoC  XT,YR,XB,VR,truo  >: 
OashLinoC  TR,X8,TL,XS, false  )S 
•nd 

•lao  Bogin 

IXB  :>  truneC  SealoOXB  ♦  Middle.*); 
IYL  :•  truneC  ScaleOYL  ♦  Middle.*); 
IXT  :•  truncC  ScaloOXT  ♦  Middle.*): 
XTR  :»  truncC  SealeOTR  ♦  Middle. T); 
MovotoC  1X8, IVL  >1 
OraotoC  XXT, IYL  ): 

OraotoC  XXT, XTR  >: 

OraotoC  XX8,XTR  )f 
OraotoC  XXB.ITL  )J 
•nd: 

•nd:  C  OraoKlioit  > 


Bogin  <  OraoLog  ) 

for  I  :■  1  to  6  do 
Bogin 
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o>  i'a  >  ’ 


X 


jaiT* 


,r 


Xaidth  S»  0.5  *  did];  Yeidth  S«  0.5  *  dyCI3: 
OraaKliait C  9ofPosSCI3,  Xaidth,  Vaidth,  true  »J 


if  I  in  Contact 
than  begin 

if  I  in  Cl, 3, S3  than  IV  :«  truneC  ScaleSVhip  a  Niddla.V  > 
also  XV  :•  truncC-ScaloaVhlp  a  Niddla.V  >S 
IX  :>  truneC  Scala*XhipCI3  a  Middle. X  >: 

MovetoC  IX, IV  )t 

IX  :»  truneC  Scale*PP3CI3.X  a  Middle. X  )j 
IV  !■  truneC  Scale*PP8CI3. V  a  Niddla.V  ): 

OraatoC  IX, IV  >: 
and 

alsa  bagin  C  put  duaay  vector  into  diaplay  buffer  > 

IX  0:  IV  :>  o:  NevetaC  IX, IV  >;  NovetoC  IX, IV  >i 
end  S 

andl 

end:  C  DraeLeg  > 

bagin  C  procedure  Vehicle  body  } 

if  PrxSensor  than  ProxiaitySensorC  PPE,PP0E  >1 

if  ConSansor  than  ContactSensorC  PPE  >; 

if  note  PrxSaneor  or  ConSansor  ) 
than  bagin 

PilNua  :«2:  StoverC  PilNua  >1 
Klneaatics: 

OisplayOb JactC  EyeSpaee, true, NuaPolys, Points, 

Polygons, Vertices  >: 

OraaLag:  C  draa  support  lags  in  topviaa  of  vohlcla  I 

StpNaol 

end: 

end:  C  Vehicle  > 

procedure  SupportPatternC  var  M  j  Matrix: 

var  Points  :  OrrayNaxPtsVector: 
var  Contact  J  Tactile 

>: 

C  Orao  support  pattorn  in  oarth  coordinato  > 

const  Middle  •  Vector!  TOO.O,  000.0,  0.0  >: 

Scale  •  15.01 

var  SupLeg 

:  arrayC1..63  of  Vector: 

CntrPrass,  Lina 

:  Vector: 

I,  J,  K,  M,  L, 

IX,  IV,  PilNua 

:  integer: 


0 


procedure  0*aeRactangleC  X.T.OIST  X  integer:  flag  X  boolean  >X 


XL.XR.TB.TT 


X  IntagarX 


begin  (  Or aeRect angls  > 


XL  X*  X  -  01  ST X  X*  !■  X  ♦  OISTi 
TB  :■  T  -  OISTI  TT  X ■  T  ♦  OISTX 
HavetoC  XL. TO  )X 
if  flag 

than  bagin  <  draa  raetangla  ) 

OraatoC  XL. TT  )!  OraatoC  XR.TT  )X 
and 

alaa  OraatoC  X.TT  >:  C  draa  triangla  > 

OraatoC  XR.TB  )X  OraatoC  XL.TB  >X 
C  Or aaRactanglo  > 


oroeadura  GatLinaaC  ftl.ft2x  Vaetari  war  Lina:  Vaetor  )i 
C  gat  coafficianta  of  equation  of  a  lino.  > 

C  lino  vector  atarts  froa  Ptl  and  anda  to  Pt2.  > 


bagin 


<  GetLinea  > 
aith  Lina  do 
begin 

X  :■  Pt2.T  -  Ptl.TX 
T  X«  Ptl.X  -  Pt2.Xi 
2  :■  Ptl.T  a  Pt2.x  -  Ptl.X  a  Pt2.YX 
andi 

C  GetLlnaa  > 


procedure  StabilityMarginC  Canter:  Vector:  KX  integer  )X 


HinOiat,  Oiat.  0.  E. 
Xint.  Tint.  Xain*  Tain 
:  real: 

IX.  IT,  I 

X  integer: 

Lina  X  Vector: 


begin 


C  StabilityHargin  » 

MinOiat  X*  1000. OX 
for  I  :■  K  doanto  1  do 
bagin 

if  I  ■  l  than  GatLinaaC  SupLogC13,SupLegCK3,Line  ) 

alaa  GatLinaaC  SupLogCX3,SupLegCI-13,Line  )X 
aith  Lina  do 
bagin 

0  i«  XOX  ♦  T*YX 
£  :•  XOC enter. v  -  vaCenter.x; 
xint  :■  c  -x*z  -  t*e  >/ox 
Tint  xa  c  -TO!  ♦  X*E  )/0X 
andi 

Diet  :•  OotProdC  Center.Line  >/aqrtC0)X 

if  Oiat  <  MinOiat  C  gat  ainiaua  distance  and  intaraaetian  » 
than  bagin 

MinOiat  X«  OiatX  Xain  X*  XintX  Tain  !•  TintX 


•ndt 

aith  Center  do  <  dree  stability  aargin  > 

begin 

IX  :*  truncC  Scale**  ♦  Middle.*  >; 

IV  :•  truncC  Sealaav  ♦  Niddla.V  ): 

MovetoC  IX. IV  ); 
and  I 

IX  :■  truncC  SealaaXain  ♦  Middla.X  >; 

IV  !»  truncC  ScalaaValn  ♦  Niddla.V  ); 

OraataC  IX. IV  >1 
(  StabllityMar gin  } 


t  procedure  SupportPattarn  body  > 

PilNua  :•  51  NaaPilC  FilNua  );  StavarC  FilNua  )! 

IX  S*  truncCMiddle.X): 

IV  S«  truncCNiddle.V):  <  cantar  of  body  in  body  coord  > 

3r  aaftec  tangleC  lX.IV.S.trua  };  <  draa  cantar  of  body  > 

aith  CntrPraas  do 
bagin 

x  :«  o.o:  v  :«  o.o:  Z  :•  i.oj 
and; 

for  i:«l  to  6  do  <  initialize  SupLag  to  gat  coefficients  of  llna  > 

aith  SupLagCIl  do  Z:at.o:  C  V  value  of  right  lag  is  positive} 

x  :■  0; 

for  IS«1  to  3  do 
bagin 

J  !«  <I-1>*&  a  19;  f  array  index  of  Points  for  tips  of  loft  lags} 

L  :•  CI-1>*2  ♦  11 
if  L  in  Contact 
than  bagin 

x  :»  x  ♦  u 

aith  SupLogCKl  do 
begin 

-V  :•  PolntsC J3. Y-HC43. v;  X  :■  PolntsCJ3.X-HC43.x; 
and; 
and! 

and; 

if  X  a  3  (  chack  covexity  of  loft  sldas  > 

than  bagin 

GetLlnesC  SupLagC33.SupLogC13.Lina  );  <  cooff.  of  a  lino  > 

0  :*  OotProdC  Lina.SuoLagCZl  >; 
if  0  >  9.0  <  lag3  is  insido  of  tha  lino  > 

than  aith  SupLogC33  do 

begin  <  raaove  lag!  in  support  pattern  > 
x  :•  x-i;  SupLegCZl.x  :•  x;  SupLegCZl.v  <■  v; 
and; 

and; 

N  *a  0; 

for  11*3  doonto  1  do 
begin 

J  !■  (I-l)*4  ♦  22:  <  array  index  of  tips  of  right  logo  > 

L  «■  1*21 
if  L  in  Contact 
than  begin 


.  KJ  *.:  ,VJ, 


1 


'!**! 


*  j»  x*it  n  :■  h^i  t 

•  ith  SutltglO  do 

bigln 

T  t>  PointsCJ3.V-HC43.Tt 
X  :■  PointsCJ3.X-HC43.Xt 
•ndt 
ondi 

ondt 
if  H  •  3 

then  begin 

GettinesC  SupLegCK3,SuptegCK-23,Llne  )t 
0  t«  3otProd(  Llne«SuptegCK-13  >t 
if  0  >  0.0 

thon  oith  SuptegCK3  do 
begin 

SuplegCK-1 3.X  t»  Xt  SupLegCK-1 3. V  :•  VI 

X  !■  K-it 

•ndt 

ondt 

IX  la  truncC  Scale*SupLegCK3.X  4  Hiddlo.X  )t 
IV  t«  truncC  Seslo«SupLogCX3. V  ♦  Niddlo.V  )t 
MovetoC  IX, IV  )i 
for  I  t«  i  to  4  do 
if  I  <-  X 

thon  bogin 

IX  t*  truncC  Scale«SuptegCI3.X  ♦  Piddlo.X  >t 

IV  t>  truncC  Scalo*$upLegCI3.V  ♦  Niddlo.V  )'t 

OraotoC  XX, IV  >t 
and 

olso  bogin  (  put  du**y  vector  into  display  buffor  > 
IX  t a  01  IV  :•  01  Hoy o t o C  XX, IV  >! 
ondt 

<  if  CntrPross  is  oassod  fro*  othor  routine,  > 

(  thon  sign  of  X  value  should  bo  changed  1 

(  before  calling  StabilityHargin  oith  X  and  V  interchanged.  1 
StabilityHarginC  CntrPross,K  )t 
StpNaa! 


ondt  <  $• 


procedure  MorkVolwoeC  var  Polygons  t  XrrayHaxPol 

var  Vortices  :  ArrayHssVI 
var  Body  :  XrraySIVoctor 
>1 


const  Hiddle  a  VoctorC  300.0,300.0,0.0  >! 

Scale  a  13.01 

til  a  2. It  L21  a  0.441  L31  a  3.01  L41  a  3.0t 

*ar 

Xeidthl ,  Vuidthl  :  resit 

I,  J,  IX,  IV,  PllNua  S  integer! 

Center  :  Vector! 


procedure  OashtinoC  xl,Vi,X2,V2t  real! 

Horizontal  t  boolean  >1 

const  H  >  5t 


(ft 
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wap 


I.  IXlt  ITl,  1X2,  I Y2 

!  intigtrt 

[v!  Int  S  real! 


begin 


If  Horizontal 

than  bagin 

1X1  S»  truneC 
ITl  :■  truneC 
and 

also  bagin 

IVl  S*  truneC 
1X1  :«  truneC 
and: 

Nowotoc  m,m  ): 


Seals  *  xi  ♦  Middle. x  >; 
Seals  an*  Middle. Y  )i 


Scats  a  xi  *  Middle. Y  ): 
Seals  a  Y1  ♦  Middle. x  ): 


Int  S»  C  X2  -  XI  )  /  Ni 
for  I  :*  1  to  N  do 
bagin 

xi  xi  ♦  int: 
if  Horizontal 

than  bagin 

1X2  :•  truneC  Scale  a  XI  *  Middle. X  ); 
if  oddC  I  >  than  OraotoC  1X2, IY1  ) 
alsa  MovetoC  1X2, IY1  »: 

and 

alsa  bagin 

XV2  :>  truneC  Seals  a  XI  ♦  Middle. V  >: 
if  oddC  Z  )  than  OraotoC  ZX1.IV2  ) 
also  MovetoC  ZX1.IV2  ); 

and: 

andl 

and:  <  DashLlne  > 


procadura  OrsaKliaitC  Canter:  Yactor:  Xoidth,Yaidth  :  real: 

OraaVoluaa  :  boolaan  )i 


war  X8,  XY,  YL,  YR 

>  raal, 

IXS,  zxr,  I YL,  XYR  :  integer: 

begin  C  OraaKliait  > 
oith  Canter  do 
bagin 

X3  :■  X  -  Xaidthl  YL  :•  Y  -  Yoidth: 

XT  :•  X  a  Xsidth:  YR  :■  Y  ♦  Yaidth: 
andl 

if  OraaVoluaa 
than  bagin 

OashllnaC  X8,TL,XT, VL,true  >1 
OashLineC  YL,XT,YR, XT, falsa  >1 
OashLinoC  XT,YR,XB,Y*,trua  X 
OashLinaC  YR.xa.YL.XS, falsa  >1 
and 

alsa  begin 

1X8  :•  truneC  SealoaxB  ♦  Middla.x): 
ITL  :■  truneC  ScalaaYL  *  Middle. T>! 


•  nd  5 

begin 


•nd; 


IXT  :•  truneC  Seale*XT  ♦  Middle. X); 
ITU  :>  truneC  Seale* YR  ♦  Middle. Y)S 
Moo# to c  rxitiYL  >: 

OraatoC  IXT.IYL  >; 

OraatoC  IXT.IYR  )J 
OrbetoC  IXB.IYR  ); 

OraatoC  1X9, IYL  )! 

•nd> 

C  Or aaKllait  ) 


(  MorkVoluao  proeadur*  body  > 

FilMua  :«  «;  NaaFilC  FilNua  )! 

aith  PolygonsC23  do  C  draa  body  > 
begin 

aith  iodyCVertieeeCStart*PolyVtees33  do 
begin 

XX  truneC  SealeOX  ♦  Middle. X); 

XV  !*  truneC  Seale*Y  ♦  Middle. Y); 

MovetoC  IX, IY  }; 

end: 

for  J  :*1  to  PolyVteee  do 

aith  BodyCVertieoeCStartaJ33  do 
begin 

IX  :>  truneC  $eale*X  ♦  Middle. X): 
IY  S«  truneC  Scale*Y  ♦  Middle. V); 
OraatoC  IX, IY  >; 
end; 

end; 

C  draa  kineaatie  liait  ) 

Xaidthl  S«  C  131+L41  )*0.5S  Yaidthl  !«  C  L11*L21  3*0.9: 
for  I  :■  1  to  6  do 
begin 

aith  Center  do 
begin 

if  I  in  Cl, 23 

then  X  :■  Xaidthl- t.4l*xhipCI3 
•lee  X  :*-Xeidthl*L41eXhipCX3; 
if  I  in  Cl, 3, S3 

then  Y  :■  Yaidthl-L21*Yhip 
else  Y  1 ■  -Yaidthl*L21-YhipJ 

end: 

C  draa  original  aorking  ooluae  > 

OraaXliaitC  Cantor , Xaidthl , Void thl , f alee) 1 
end: 

StpNea: 

C  MorkVoluao  > 


'*  A 

V 
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t a**************** ******************************************** **********> 
<  FIX*:  TEREST.P1S  > 

(  This  fila  is  for  tsrrain  astiaation  through  linara  ragrassion.  > 

i  > 

( «****«*«« a* ****************************************** a*****************) 


Tincluda  Global! 
tincluda  Extarnalf 


procadura  EstiaataPlanaC  war  SupportPlana  :  Plana! 

var  PPSE  :  Array6Vactor 

>i 


bagin 

t  Coo 


Xbar.  Tbar.  Zbar. 
Nuoli  Nua2.  Nu*3» 
Oanl.  0an2. 

TRbar.  ZRbar. 

10.  11.  12.  13.  11. 
1.  0 .  C.  0.  N 

:  raali 

I  :  intagar! 

r* ,  zr 

:  Array6! 


N  !*  6.0!  Xbar  !■  0.0!  Tbar 
for  I  fa  1  to  6  do 
bagin 

Xbar  fa  Xbar  ♦  PPSECI3.X! 
Vbar  !«  Tbar  ♦  PPSSCZ3.T! 
Zbar  !•  Zbar  ♦  PPSECX3.Z! 


SS  > 

Tbar  fa  0.0! 


Zbar  f«  0.0! 


and! 

Xbar  fa  Xbar/N ! 
Tbar  :*  Tbar/Ni 
Zbar  !•  Zbar/N i 


{  coaputa  10.  11.  12.  and  13  > 

Nual  t*  0.0!  Oanl  !•  0.0!  Nua2  !*  0.0! 
for  I  !•  1  to  i  00 

aith  PPSECX3  do 
bagin 

Nual  !■  Nual  ♦  (  X  -  Xbar  )  *  <  Z 

Oanl  !■  Oanl  ♦  <  X  -  Xbar  )  *  C  X 

Nua2  !•  Nuo2  ♦  C  X  -  Xbar  >  *  <  T 

and! 


Zbar  ) ! 
Xbar  >i 
Tbar  >! 


11  !>  Nual/Oanli 

12  !•  Nua2/0anli 

10  !>  Zbar  -  11  •  Xbar! 

13  fa  Tbar  -  12  a  Xbar! 


<  eoaputa  TRbar  and  ZRbar  > 
TRbar  !■  0.0!  ZRbar  fa  0.0! 
for  I  fa  1  to  6  do 

aith  PPS2CX3  do 


:«  y  -  42  -  43  *  x; 
YRbar  YRbar  ♦  YRCX35 
ZRC 13  :«  Z  -  40  -  41  *  x; 
ZRbar  :»  ZRbar  *  ZRCI35 
and ; 


YRbar  :«  YRbar/N; 

ZRbar  :>  ZRbar/N5 

Mu»3  :*  0.05  OanZ  :«  0.05 
♦  or  I  :•  1  to  Ho 
bagin 

Hu»3  :*  Nua3  ♦  <  YRCI3  -  YRbar  >  *  C  ZRCX3  -  ZRbar  >5 
0an2  S»  OanZ  ♦  (  YRCZ3  -  YRbar  )  *  (  YRCI3  -  YRbar  )  i 
and; 


44  Nu«3/Can25 

t  gat  plana  coafficianta  :  »*X  ♦  S*Y  ♦  C*Z  ♦  D  -  0.0  > 

4  J*  -41  ♦  44*435  3  5«  -445  C  S«  15  0  S*  -40  ♦  42*445 

M  :■  sqrtC  4*4  «  6*8  ♦  C*C  )5 

aith  Support PI  ana  do 
bagin 

•ith  Uni t Nor *a 1  do  <  Noraal  vactor  always  goas  poaitiva  Z  diraction  > 
bagin  X  :■  4/M;  Y  :«  8/M;  Z  :■  C/M;  and; 

Oist  5*  0/4 5  (  if  plana  is  abowa  origin*  Oist  <0.0  > 

<  if  Plano  i*  baloa  origin*  Oiat  >0.0  > 

and; 

and5  (  SatiaataPlana  » 


procadura  TarrainCoordC  war  Hi 

T, 

INVT  * 

G  • 

XnvG  ;  Matrix; 
war  Etl  :  raal; 
war  SPE, 

SP8  :  Plan* 

>; 


<  rotation  angl*  fro*  XE  to  XT  > 


bagin 


Bata*  <  rotation 

N, 

SBata*  C3ata*  SEtal.  CEtal 
:  roal; 


(  gat  poaition  woetor  of  T  1 

TC43.X  ;•  MC43.X5  TC43.Y  5»  MC43.Y5 

aith  SPE  do 

•ith  Uni tNoraal  do 
bagin 

TC43.2  5*  -<  X*MC4 3. X  ♦  Y*MC43.Y  ♦  Oiat  >/  Z5 
if  aba<  1.0  -  Z  )  <  0.000001 

than  bagin  C  if  no  alopa*  than  tarrain  eoord  > 
{  la  aa*a  aa  oarth  coord  > 

Sata  s«  0.05  Etal  :*  0.05 


else 


end 

begin  {  if  slope*  get  sieve  angle  and  > 
Bets  :■  Stan2(  x*-V)l  <  rotation  angles  ) 
Etal  :>  Sre Cost  1  >t 
ondt 


CEtsl 


end: 

<  get  rotation  satria  of  T  > 
SBeta  :■  Sint  Sets  ):  SEtal 

CBota  i>  Cost  Beta  )• 
elth  Ten  do 
begin 

x  :«  CBeta:  V  : 
end: 

eith  TC23  do 
begin 

X  :«  -SBeta  *  CEtai: 
and: 

eith  TC33  do 
begin 

X  :»  SBeta  *  SEtal : 
end: 

Matlnverset  T.INVT  ): 

NatHultt  INVT *H«G  )1 
Matlnverset  G»INVG  )! 


i*  Sint  etal  ): 
:■  Cost  Etal  >: 


•  SBeta:  z  :>  o.o: 


T  :■  CBeta  a  CEtai:  Z  :■  SEtal: 


V  :— CBeta  *  SEtal:  Z  :■  CEtai: 


<  get 
tget 
t  get 


PlaneTranaforat  SPE*H,SP3  >i 


endi 


(  TerrainCoord  I 


inverse  eatria  af  T  ) 

transfers  eatria  G  fraa  body  to  terrain  >: 
inverse  eatria  of  G  > 

<  transfers  support  plane  > 

<  in  body  eeord  > 


Ctnomain) 

(Soon) 

<***************************#********•*********«***«****«**********«****> 


<  aii«:  legcoro.pas  > 

€  This  fils  is  for  log  coordination  sith  computation  of  optimal  psriod  > 

<  and  ths  change  of  the  position  and  size  of  the  constrained  eorking  > 

(  volumes.  > 

C  » 


{•****************************************** ********************** ***»«*> 


linclude  Global! 

{include  External! 

procedure  Getlnter sectionC  var  ClipXTf 

ClipXB. 

ClipTL. 

ClipTR. 

XI. 

Tl. 

X2. 

T2  :  real! 
var  Error  s  integer 
)  ! 


{  XI. Tl  are  the  position  outside  kinematic  limit  > 

<  X2.T2  are  the  present  foot  position  > 

<  if  X2.T2  are  outside  kinematic  limit,  return  *rror*l  > 

(  XI. Tl  return  the  intersection  point  in  body  coordinato  1' 


label 

10! 

type 

OutCode  «  set  of 

l..*S 

var 

Emptyl.  Empty!  ! 

boolean! 

C,  Cl.  C2  : 

OutCode! 

X,  T  :  real! 

procedure  CodoC  X.T  :  real! 

var  C  >  GutCodei 
var  Empty  !  boolean 
) ! 

begin 

C  :■  Cl! 
if  X  <  ClipXS 

then  C  !»  C  *  3 

else  if  X  >  ClipXT  then  C  !■  C  3  3! 
if  T  <  ClipTR 

then  C  *■  C  ♦  C  2  3 

else  if  T  >  ClipTL  then  C  *■  C  ♦  C  1  3! 
if  C  •  Cl  then  Empty  *■  true 

else  Empty  i«  false! 

end!  (  Code  > 
begin 

CodeC  Xl.Tl.Cl.Emptyl  )! 

CodoC  X2.T2.C2. Empty!  )i 
if  not  C  Empty 1  or  Empty!  ) 
then 

if  C  Cl  *  C2  )  <>  C 3 
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$ 

& 


f* 


I 


10: 

■»nd; 


then  begin 

Error  :•  1:  goto  10; 

•  nd 

•Iso  Error  :■  Ot 

•hlloC  Cl  <>  Cl  )  or  C  C2  <>  Cl  )  do 
begin 

if  (  Cl  *  C2  )  <>  Cl  th«n  goto  10: 

C  :«  CIS  If  C  ■  Cl  then  C  :•  C2J 
if  4  in  C 

thon  bogin 

v  :■  ti  ♦  CY2-m*cciipxe-xi>/cx2-xi>* 

x  ciipXBs 

ontf 

•Iso  if  3  in  C 

thon  bogin 

y  :•  ri  ♦  CY2-n»*cciipxt-xi)/cx2-xn: 

X  :«  ClipXTJ 

snd 

•Iso  if  1  in  C 

thon  b*gin 

X  :•  XI  ♦  <X2-Xl>OtClipYL-Yl)/(Y2-Yl>: 

V  :»  ClipYLJ 
•nd 

•Iso  if  2  in  C 

thon  bogin 

X  !•  XI  ♦  (X2-Xl)0(ClipTR-Yl )/(Y2“ Y1 ) I 

Y  S«  ClipYR! 

•ndi 

if  C  •  Cl 


thon 

begin 
XI  t« 
•nd 

Xt 

Y1 

:»  y; 

Coast 

X1«Y1 ,C1* Eaptyl 

•Iso 

bogin 
X2  :• 
•nd: 

x: 

Y2 

:■  y: 

Ced«C 

X2(Y2,C2*€opty2 

)i 

•nd; 


procedure  OptlnalPeriodt  var  OodyTransRste, 

BodyRotateRate  :  Vector: 
•or  RefPosS, 

PPS  :  *rray6Veetor : 
var  ds* 
dy» 

LPhaso  :  Srrayd; 
var  Contact  :  Tactile: 
var  Bata* 

Period  :  real 


label  201 

const  diddle  ■  Vector<  300.0.800.0,0.0  >1 
Seals  •  1S.0: 
daxPeried  •  10000.0: 
dinPoriod  ■  0*031 
InnarV*!  «  0.051 
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OuterVel  ■  0.2 


war  XPoot,  YPoot,  ZPoot • 

XFootVel,  VFoo  t Vel  •  ZFootVel, 
Xeidth,  Yeldth, 

ClipXT ,  ClipXB,  ClipYL,  ClipYR  > 
Sperlod, 

FWOPerlod,  BWOPerlod, 
FWOSPeriod,  BUOSPeriod, 

XXint .  K Yint ,  XXintl,  KYintl, 
VBL.  * 

Yl,  X2,  Y3 »  X4 
:  riii; 

m*  xvi, 

1X2.  I V2f 
FUNvii 
I.  J*  J1 

:  Integer; 

RightLeg 

;  boolean; 


procedure  MarginC  war  XPoot, YPoot, ZPoot, 

XFootVel, YPoot Vel, ZPoot Vel  :  real  ); 

label  200: 

const  LongTlae  ■  1000000. 0. 

MinVel  *  O.OOOOi; 

war  FootVel, 

XMargin,  SPTlae,  ZTiae, 

SPhase* 

Xlnti  Vint 

:  real; 

Error  :  integer; 

begin  <  procedure  Margin  body  >  , 

if  RightLeg  <  transfers  right  to  left  > 

then  begin 

YPoot  *•  -YFootl 
YPootvel  :•  -YPaotVel; 
end; 

PootVel  :>  sgrt (  XPoot YelsxPootVel  ♦  YPoot VelOYPootVel  a  ZPoot VaiaZPootVel  >; 
if  PootVel  <  MinVel  <  if  foot  weloeity  is  alsost  zerot  ) 

than  begin  <  then  set  support  period  to  10000.0  > 

PUOPoriod  ;•  MaaPerlodl 
BWOPerlod  J»  MasPerlodt 

if  PUOPoriod  <  PUOSPorlod  then  PUOSPeriod  *•  PUOPerlodT 

if  BUOPeriod  <  BUOSPeriod  then  BUOSPeriod  :•  BWOPerlod; 

goto  200; 
end; 

<  define  an  ioaginary  point  outside  kinesotic  11*11  to  cooputo  intersection  > 
Xlnt  ;•  XPoot  •  XPootVel  e  LongTlae; 

Tint  1«  YPoot  a  YPootVel  O  LongTlae; 


I 

t 

i 


s 


I 

* 

« 

* 

> 

t 


1 

\ 

i 


GetintersectionC  C lip* T,C 1 ip xe • C lipYL . C lip TR t Hint, Tint ,  X Foot ,Y Foot , Error  )j 

if  Error  *  1  thon  begin 

FUOSPeriod  !•  MinPeriod; 

BUOSPorlod  S«  MinPeriod: 
goto  200S 
•nd  • 

XMargin  !■  sqrt<  <Xint-XPoot)*<Xlnt-XPoot>  ♦  <Tint-YFoot>e<Yint-YFoot J  >; 

<  supporting  tiao  to  roach  kinoaatic  Xiait  voluoe  ) 
eootVel  r«  sqr  t(  XFoot VeloxFootVel  ♦  YFootVelOTFootVel  )! 

if  PootVol  <  MinVel  than  SPTiao  !*  100000*0  a  XMargin  <  support  tiao  > 

olso  SPTiao  :■  XMargin  /  FootVel:  (  for  «  and  y  val  > 

if  ZPootVol  >  0.0  {  support  tiao  for  x  velocity  ) 

thon  ZTiae  :»  C  Zaax  -  ZPoot  )  /  ZPootVol 
olso  if  ZPootVol  <  0*0 

thon  ZTiae  !•  -C  ZPoot  -  Zain  >  /  ZPootVol 
olso  ZTiae  !■  SPTiao; 
if  SPTiao  >  ZTiae  thon  SPTiao  !*  ZTiae; 

<  support  period  for  support  logs  > 

SPhase  :«  LPhasoCXl/Seta; 

if  1.0-SPhase  <  0.00001 

thon  FUOPoriod  :>  100000. OaSPTlao 
olso  FUOPoriod  !•  SPTiao/d.O-SPhaso); 
if  SPhaso  <  0.00001 

thon  BWOPoriod  :■  100000. OaSPTlao 
olso  BUOPorled  :«  SPTiao/SPhass; 


t  take  ainiaua  period  of  support  periods  for  foraard  sequencing  > 
if  PuOPoriod  <  FUOSPeriod 
thon  begin 

FUOSPeriod  :»  PuOPoriod; 

J  :*  I’;  (  STORE  LEO  NUMBER  > 

KXint  !*  Xint; 

if  Rlghtleg  thon  XTint  :•  -Tint 

olso  XTint  :■  Tint  ; 

end;  • 

(  take  ainiaua  period  of  support  periods  for  backaard  sequencing  > 
if  BuOPoriod  <  BWOSPeriod 
thon  begin 

BUOSPorlod  :■  BuOPoriod; 

KXintl  :■  Xint  ; 

J1  :>  i; 

if  Rlghtleg  thon  KYlntl  :■  -Tint 

olso  XYintl  :•  Tint  : 

end : 

zoo: 

end:  (  Margin  } 


begin  <  procedure  OptiaalPerlod  oody  > 

PUOSPoriod  :*  MaxPeriod:  <  initialize  support  period  1 
BUOSPorlod  :■  MaxPeriod; 


for  I  :■  l  to  6  do 

if  I  in  Contact 

thon  begin 


x«i*th  :•  0.5  *  d*cij;  reidth  :» 
if  I  in  0*3*53  thon  RightLog  :» 

oli*  RightLog  :* 
(  doflno  kineaatic  licit  of  each 
ClipXT  :«  RefPosBCIJ.X  ♦  Xoidtht 
ClipXS  t«  RefPosBCIJ.X  -  Xoidthi 
ciipYL  :»  RofPo«acz3.r  ♦  Told th: 
ClipYR  to  Ref PosSCXJ. Y  -  VoidthS 
if  RightLog  thon 
bogin 

CiipYL  :•  -RefPosBCIJ.T 
ClipYR  t"  -RefPosBCI J.Y 
and: 


0.5  *  dyCIi; 
falsa 

true: 
lag  > 


a  Vaidthi 
-  Vaidth: 


(  cooputa  naa  period  using  naa  foot  velocity  ) 

XPoot  :>  PPBCX3.X: 

YPoot  »■  PPBCX3.Y; 

ZPoot  to  PPBCI3.X: 

XPootVal  to-BodyTransRata.X  ♦  BodyRotataReta. Z  *  YPoot 

-  BodyRotataReta. V  a  ZPoot: 

VPootVal  :>-BodyTransRate. Y  -  BodyRotataReta. Z  *  XPoot 
a  SodyRotataRata.X  *  ZPoot: 

ZPoatVal  to-BodyTransRato.Z  ♦  BodyRotataReta. Y  a  XPoot 

-  SodyRotataRata.X  •  YPoot: 

PerginC  XPaot*VPoot,ZPoot*XPootVel*VPootVel*ZPootVel  ): 
and: 

(  consider  only  longitudinal  or  lateral  velocity  > 

<  to  choose  betaeen  feroard  and  backward  gaits  > 
aith  BodyTransRate  do 

Vel  :«  sqrtc  x*x«veY>: 

if  (  Vel  <■  OutorVol)  and  (  abs(  BodyTransRate. X  )  <  InnerVel  > 
then  begin  (  pure  turn-in-place  case  > 

if  Period  >  0.0 

then  SPoriod  :■  PWOSPeriod 
else  SPoriod  I*  -SUOSPerlod: 

end 

else  if  Vel  <■  OutorVol 

then  begin  (  gait  change  region  > 

if  SodyTransRate.x  >  0.0 

then  SPoriod  :•  PWOSPeriod 
else  SPoriod  :•  -BWOSPerlod: 

end 

else  if  Period  >  0.0  (  outside  gait  change  region  > 

then  SPoriod  :■  PWOSPeriod 
else  SPoriod  :■  -SWOSPeriodl 

if  SPoriod  <  0.0 

then  begin 

J  :■  Ji:  Mint  :•  KXlntlf  Mint  :•  xvinti: 
end: 


PilNua  :•  7:  NaaPilC  PllWue  >:  St evert  PllMua  >: 


Q 


Period  :»  SPeriod  /  Seta;  C  conpute  period  ) 

if  (SPirlod  ■  NaxPeriod)  <  if  vehicle  velocity  is  alnost  sere  > 

or  CSPeriod  »  -NaxPeriod)  <  skip  draaing  kineeatic  Margin  ) 

or  CSPeriod  •  MinPeriod  ) 
or  (  SPeriod  •  -MinPeriod  ) 

then  begin  {  put  dunoy  vectors  to  display  buffer  > 

:xi  :>  o:  ivi  :■  os  1x2  :»  o;  IV2  :■  o: 

MovetoC  IX1.IY1  )S  MovetoC  1X2, IY2  );  goto  20: 
end: 

<  drao  line  fron  footposition  to  intersection  point  1 
1X1  :•  truncC  ScaledPPSC J3.X  ♦  Middle. X  >: 

IT1  :•  truncC  ScaloSPPSC J3.Y  ♦  Middle. V  ): 

1X2  :■  truncC  ScaleOKXint  ♦  Middle. X  >: 

It 2  :•  truncC  Scale*KYint  ♦  Middle. Y  >; 

MovetoC  IX1,IV1  >:  OraotoC  ZX2.ZY2  >; 

20:  StpNan: 

end:  C  Op tiaalPer iod  > 


procedure  LegCoorolnationC  var  H, 

IN VN  :  Matrix: 
var  SPS, 

SPS  :  Plane: 
var  SodyTransRate* 

SodyRotateRate  1  Vector: 

PPB  :  irreydVector : 
var  dx, 
dy. 

RPhase, 

LPhase  :  trrayi: 
var  Contact  :  Tactile: 


dxaax,  dxain, 
dynax,  dynin, 
0T, 

Seta* 

Phase* 

Period* 

SloadoonPactor 


:  real 


const  MinTRTiMO  ■  0.91 

var  I  :  integer: 

TRTine  t  real: 

procedure  NeeCuv: 

<  get  neo  dinensions  of  constrained  sorking  voluaoo 
C  depending  on  the  foot  velocities 

var  I  :  integer: 

Xoidth*  Yoidth, 

XPoot*  YPoot*  ZPoot* 

XPootVel*  YPootVel,  IPootVel 


ruls 

vector 


begin  <  NeeCUV  ) 


tor  J 


:•  I  to  6  do 
begin 

if  not  Cl  in  Contact) 


then  begin 

Xfoot  :>  Ref PosBCI 3. x ; 
TFoot  :«  RtfPos8CI3.TJ 
ZFoot  J*  Ref PosBCI). Z; 


<  get  foot  velocity  components  > 
XFootVel  :*  abs(  -flodyTr ansRate. x 


eBodyRotateRate.Z  *  TFoot 
-Body Rot a to Rate. T  *  ZFoot  ); 
TFootVel  :»  abs<  -BodyTr ansRate. T 

-SodyRo t ataRate. Z  *  XFoot 
*3odyRotateRete.X  e  ZFoot  ); 


<  get  deeired  a  and  y  dimensions  of  CUV  > 
if  XFootVel  <  Vein 

then  daCX3  :»  dxein 
else  if  XFootVel  >  Vaas 

then  daCI3  :»  disas 

else  daCX3  :«  (  daeaa-dxain  > 


if  TFootVel  <  Vain 

then  dyCIl  i*  dyain 
else  if  TFootVel  >  Vaas 
then  dyCIJ 


e  C  XFootVel-Vain  ) 
/  C  Vaas-Vain  ) 

*  daain) 


then  dyCIJ  !•  dyaaa 

else  dyCZ 3  «»  C  dyaas-dyeln  ) 

*  <  TFootVel -Vain  *) 
/  C  Vaas-Vain) 

♦  dyain t 


Xaidth  :■  0.5  *  dxCZ3l  Taidth  :•  0.5  *  dyCIJ  J 


<  get  nee  center  positions  of  CMV  > 
eith  Refpos6CX3  do 
begin 

if  Z  in  Cl. 23 

then  X  *•  XhipCIS  ♦  XL  -  Xaidth 
else  if  X  in  C5.63 

then  X  :•  XhipCX3  -  XL  e  Xaidth 
else  X  :«  XhipCI3) 

if  X  in  Cl. 3. 53 

then  T  :■  Thip  ♦  VL  -  Taidth 
else  T  :«-Thip  -  TL  ♦  Taidth) 

Z  :•  -NoainalHelght: 
end) 


if  I  in  C3.S3  <  change  the  center  of  constrained  > 
<  eorking  voluae  of  aiddle  legs  > 
then  begin 

Tea  :■  RefPosBCIl)  Tea.Z  *■  0.0) 
VectTransferaC  H, Tee. Tea  )) 


iS& 

I'M 

•’T' 


V«*  Vital1 VJtfWl%aV!fc  V’tio^V^ **  *»*  i 


1 


■  ith  SPE  do 

■  ith  UnitNoroal  do 
bogin 

Too. I  S*  -(XOToo.X  o  TOToo.T 
♦  Oist)/ZS 

VoetTransforoC  IMVH.Too.Too  >; 

RofPosBCXX.X  *•  Too.XS 

•ndi 

(  chock  boundary  of  original  oorklng  voluoo  > 
•ith  Rofpoobcn  do 
bogin 

if  X  -  Xoidth  <  -XL 

than  X  :>  Xoidth  -  XL; 
if  X  ♦  Xoidth  >  XL 

then  X  :•  XL  -  Xoidth; 

ontf ; 

•nd. 


for  1  :■  1  to  &  do 
bogin 

<  updato  roforonco  position 
•ith  SPE  do 

•ith  UnitNoroal  do 

RofPosBCXl.Z  :«  -{ 


(  for  1  loop  > 


♦  01st  >  /  z; 


icii.x  ♦  voRofPosBcn.r 


<  NooCWV  > 


bogin  <  LogCoordination  > 


NooCMV; 


<  the  noo  constrained  oorklng  woluoo  > 


OptioalPoriodC  Body Translate .Body RotatoR ate .Rof Pas B.PPBt 
die  «dy*LPhsso*Contact»  Sots*  Ported); 

TRTloo  :■  (  1.0  -  Bota  )  *  absC  Poriod  >; 
if  TRTloo  <  NinTRTloo 
thon  bogin 

SloodoonPactor  »■  TRTloo  /  'UnTRTioo; 
oritelnC'Sloodoon  Pactor  ■  *,  SloodoonPac toriSt 3  >; 
TRTloo  !«  NinTRTioo; 

Poriod  :•  Sign<  Poriod  )  •  TRTloo  /  (1.0  -  Sots  >1 
•nd 

•Iso  SloodoonPactor  *•  1.0; 

<  updato  phase  variables  > 

Phase  J«  Phase  ♦  0T  /  Period  ♦  1.01 
Phase  *■  Phase  -  trunct  Phase  )l 
for  X  :•  1  to  6  do  <  cooputo  log  phase  variable  > 
bogin 

LPhasoCXl  !■  Phase  -  RPhasoCXl  ♦  1.0J 
LPhaseCI)  :•  LPhasoCX)  -  trunc(  LPhasoCX)  >S 
•ndl 

•nd;  <  LogCoordination  > 


% 

ft 


■■■■■■■■■■■■■■■■■■■■■■■pi 


(tnoaaln) 

<toan> 

<*** ******************************** *********•****•***********•***•*•***) 
(  Flu:  BOOT  .PAS  > 

<  This  fils  is  for  ths  body  sot-point  generation.  > 

<  > 

I  ***************** ****** ******** a***************************************) 

Tinclude  Global: 
tinclude  External; 

procadura  3odySerwo(  war  M, 

INVH  :  Hatrix: 
war  SP9. 

SP5  :  Plana: 
war  9odyTransRate( 

Body Rota teRate  :  Vactor: 
war  RofPosB* 

PPB  :  Array&Vector ; 
war  da* 
dy* 

RPhasa* 

LPhase  :  Array6: 
war  Contact  :  Tactile: 
war  Vaax>  Vain* 
daaaa*daain. 
dy*as*dyain 
OT, 

Bata* 

Phase* 

Period* 

Etal  * 

NVELK, 

NVELT, 

NRVELZ  :  real 


SloadoanPactor 
:  real: 


war  SodyTransRata* 
BodyRotatoRat* 


Vector 


(  this  is  for  eutoaatic  body  attitude  and  altitude  control 


0*5  e  0.7054; 

HinAngle  •  0.00001: 
MaxAngl*  ■  0*7: 
MinHeight  ■  4.0: 

OVSLX*  OVELV,  ORVELZ. 
R. 

Height* 

Gaaaa* 

Again* 


(  *5  degree  in  radian  > 

<  0  degree  in  radian  > 

<  30  degree  in  radian  > 

<  body  height  for  45  degroe  slop*  > 

(  velocity  deriwatiw*  terns  > 


<  angle  betaean  noraal  vectors  of  > 

(  body  and  support  Planes  > 

(  filter  gain  for  attitude  control  > 
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Etal, 

TineConst 

:  real; 

I 

t  integer! 
K 

:  Vector; 
DSPE «  06PB 


:  Plana; 


<  desired  oody.  attitude  angle  » 

<  tlaa  const  for  rotational  velocity  > 

(  vector  for  rotating  axis  > 

<  desired  Oody  plane  in  earth  snd  in  body  > 


begin  <  BodyRates  > 

If  Etal  <  Miningle  (  Oody  plane  is  parallel  to  support  plane  1 

then  eith  SP3  do 

eith  UnitNornal  do 
begin 

Height  :■  NoeinalHoight ; 

Kgain  S»  5.0; 

M  :«  sqrtC  XSX  ♦  TOY  >: 

if  N  ■  0.0  t  get  rotation  axis  vector  K  > 

then  begin  K.x  J *  0.0;  K.Y  ;•  D.ot  end 

else  begin  K.X  :■  -Y/n;  K.Y  ;•  t/nx  end; 

Ganna  :«  ircCosC  l  >;  <  0.0  <  Ganna  <  PX/2  > 

BodyRotateRate.X  S*  Kgain  *  (  K.X  *  Ganna  )l 

BodyRotat eRate. Y  :■  Kgain  a  (  K.Y  •  Ganna  >; 

Body Trans Rate. 2  t«  Kgain  a  C  MoninalHeight  -  01st  )t 
end 

else  begin  (  body  plane  is  not  parallel  to  support  plane  > 

Etal  :■  Miningle  ♦  <  Nisingle  -  Miningle  >  /  C  0*5  -  Miningle  ) 
*  (  Etal  -  Miningle  )l 

Height  ;■  NoeinalHoight  -  C  MoninalHeight  -  MinHeight  > 

/  (  C*S  -  Nlningle  >  *  (  Etal  -  Nlningle  )t 
Kgain  :•  5.0: 
nith  SPE  do 

aith  UnitNornal  do 

N  :>  sqrtC  xex  ♦  Y*V  )i 

aith  0BPE  do  (  desired  body  plane  in  earth  coord  > 

nith  UnitNornal  do 
begin 

X  :»  SPE. UnitNornal. X  /NO  Sin(  Eta2  )1 
Y  :«  SPE. UnitNornal. Y  /  N  *  Sint  Eta2  ); 

Z  S-  Cost  Etal  >: 
end; 

PlaneTransforaC  OBPE.M.DBPB  >; 
eith  0 BPS  do 

aith  UnltNoraal  do 
begin 

N  !■  sqrtC  X*X  ♦  Y*Y  )i 

if  N  ■  0.0  <  get  rotation  avis  voctor  K  > 

then  begin  K.x  ;»  0.0;  K.Y  :•  0.0;  end 

else  begin  K.x  :■  -Y/N!  K.Y  :■  t/nx  end; 

Ganna  !■  ircCosC  2  )t  (  0.0  <  Ganna  <  PX/2  > 

BodyRotateRate.X  :•  Kgain  e  (  K.x  a  Ganaa  >1 

BodyRotateRate.Y  ;«  Kgain  a  (  K.Y  a  Ganaa  >; 
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3ody TransRata. Z  S«  Kgain  *  {  Haight  -  SPS.Oist  > 
and: 


•ndi 


TiaaConst  !■  0.5; 

<  filtarad  rata  coooands  > 

OVELX  :«  (NVELX  -  BodyTransRata. X)  /  TiaaConst;  <  longitudinal  accalaration  ) 
DVELT  :■  (NVELT  -  BodyTr ansRata. T )  /  TiaaConst;  <  lataral  accalaration  1 
ORVELZ  :»  CNRVELZ  -  Body Ro tataRata. Z )  /  TiaaConst;  <  turning  accalaration  > 

BodyTransRata. X  :*  OVELXaOT  ♦  BodyTr ansRata. X :  C  logitudinal  walocity  > 

3ody Tr ansRata. T  :■  DVELT*0T  ♦  Body TransRata. Yt  (  lataral  yalocity  > 

Body RotataRata. Z  S«  OR VELZ*0T  ♦  Body RotataRata.Z ;  (  turning  rata  > 

and;  (  SodyRatas  > 


procadura  Oacalara tion<  war  Sloadoanract or 

yar  Body TransRata* 
.BodyRotataRata 

>; 


raal; 

Vactor 


bagln  <  Oacalaration  > 

-oith  BodyTransRata  do 
bogin 

x  :•  SloodoonFact or  *  x: 

Y  :»  SloodoonFaetor  *  Y: 
Z  s«  SloodoonFaetor  *  Z; 
and: 

oith  BodyRotataRata  do 
bogin 

x  :«  SloodoonFaetor  »  x; 

Y  :■  SloodoonFaetor  *  v; 
Z  :■  SloodoonFaetor  *  Z: 
and; 

and;  <  Oacalaration  1 


procadura  UpdatsHC 

var  BodyTransRata* 
BodyRotataRata 


yar  H*  INVH 


); 


vactor; 

Matrix 


yar 


OistX.  OistY,  OistZ* 
OoltaX*  OaltaY*  OaltaZ 
:  raal: 


I 


intagar: 


<  dlffarantial  translations 
{  dlffarantial  rotations 


> 

> 


OaltaH*  HOalta  <  diffarantlal  aatrlx  of  H  > 

:  Matrix ; 

bogin  <  UpdataH  > 

(  gat  diffarantlal  changa  of  body  > 

■ith  BodyTransRato  do 
bogin 

Oistx  :*  x  •  cr;  Oistr  :■  f  *  or;  OistZ  :*  z  ♦  or; 
and; 

•1th  BodyRotataRata  do 
bogin 

Oaltax  :>  x  «  0T;  Oaltaf  :•  T  *  OT;  OaltaZ  :»  Z  *  OT: 
and; 

<  gat  diffarantlal  changa  of  H  > 

•ith  HOaltaCll  do 

bogin  X  :»  0*0:  T  :■  OaltaZ:  Z  ;■  -Oaltaf:  and: 

•ith  HOaltaC23  do 

bogin  X  :•  -OaltaZ:  T  :•  0.0:  Z  :■  OaltaX;  and: 

•ith  H0sltaC33  do 

bagin  X  J*  Oaltaf;  f  :•  -Oaltax:  Z  t»  0.0:  and; 
aith  HOaltaCO  do 

bagin  X  :■  Oistx;  f  :■  Oistf:  Z  :■  OistZ:  and: 

NatMultC  h, HOalta, OaltaH  >;  <  OaltaH  ■  H  *  HOalta  > 

<  gat  updated  H  > 

for  I  :■  1  to  4  do  C  H  ■  H  a  OaltaH  > 

•ith  HCI1  do 

if  I  <>  4 

than  bagin 

x  :•  x  ♦  OaltaHCn.x; 

f  :•  f  ♦  oaltaHcn.f: 

Z  :•  z  ♦  oaitaHcn.z; 

and 

also  bagin 

x  :•  OaltaHCZS.x: 
r  :■  DaltaHCXl.f : 
z  :•  OaitaHcn.z: 
and: 

Orthogonal iz at  ion (  H  ) : 

Matlnvertei  M» Invh  ): 

andl  (  UpdataH  > 


bagin  (  BodyServo  > 

BodyRatasC  BodyTransRato •  BodyRotataRata  >: 

LagCooroination(  H.IHVH, SPB*SPE* BodyTransRato • Body Rata toRato, 

Ref PosB*FP8 «dx*4y* Rohase*iPhaso* Contact »Vaax* 

Vain* dxaax * dxain* dyaax *dyain* 

OT* Bata* Phaso* Par  loo* SloadoanPactor  >: 

if  SloadoanPactor  <  1.0 

than  OacolorationC  SloadoanPactor* 

BodyTransRato* 

BodyRotataRata  >: 

■ritolni  Period:8:3*'  ',BodyTr ansRata.X: S: 3* *  '*BodyTransRate.f:8:3* 
*  *, BodyRotataRata. Z:8:3>: 

UpdatoHC  BodyTransRato*  BodyRotataRata*  M,  XN¥H  >; 


andl 


(  BodySarvo  > 


T7*JTZVJ7TT.  TT?m 

Ss 

£ 


VI.WWJW 

fe 


(tooniin) 

(toon) 

C********* ******** **************************** **************************> 
(  Fils:  AS VINT* PAS  > 

t  This  fils  is  for  initialization  of  paraaotsrs  and  vshiels  notion.  > 

<  > 

{******* ********** ************ ******************** ************ **********> 

fincludo  global: 

Tincluds  Extsrnal: 


procsdurs  ASVINITC 


var  H* 

INVH  :  Matrix: 
var  dx  i 

dy  :  Array6; 
var  Contact  :  Tactlls: 
var  dxain. 
dy«in» 

OT, 

FootLift  :  rsal; 
var  Bsta.Mods  :  intsgsr: 
var  SsfPosBt 
FPOE* 

FPSE. 

FP8  :  Array6V*ctor : 
var  EysSpacs  :  Matrix*: 
var  NuaPoly*  :  intsgsr: 
var  Body  :  Array52 Vsctor : 
var  Paints  :  ArrayMaxPtsVsctor ; 
var  Polygons  i  ArrayMaxPolys*olygon: 
var  Pianos  :  ArrayMaxPolysOnsPlans : 
var  Vsrticss  :  ArrayMaxVtcssIntsgsr 


i: 

var  I«  J*  FilNua  :  intsgsr: 

«  :  tsxt; 

Filsnan*  :  String: 

G  :  fils  of  OnsPlans: 

PPE  :  Vsctor: 

Xaidth,  Tsidth 

:  rsal: 


bsgin  (  AS VI NI T  > 

{  fix  dt  for  non-rsal  tins  opsration  } 

DT  :«  o.os; 

Foottift  :«  1.0: 

BotaMods  :«  *: 

dxain  :*  0.25s  dysin  3»  0.25: 

<  initializs  rsfsrsncs  positions  > 

Xaidth  :»  0.5  *  dxain:  Tsidth  l»  0.5  *  dyain: 

for  I  :■  1  to  6  do 
bsgin 

sith  RsfPosBCIl  do 
bsgin 

if  I  in  Cl,  23 

thsn  X  :«  XhipCIJ  ♦  XL  -  Xaidth 
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L 


W5  *.J  A  fg  v:  TO  A‘>.'  TOT?  TE  V.'W^TO  m  ••*  re  l*  ^  t  vwwwwwntWH  ~ 


else  if  !  in  C5*  63 

then  X  S«  XhipCIl  -  XL  ♦  Xmidth 

•is*  x  :«  xhipcn; 

if  I  in  Cl. 3. 53 

then  Y  !»  Thip  ♦  YL  -  Yeidth 
•Is*  Y  :«-Yhip  -  YL  ♦  Yeidth ; 

Z  :•  -NominalHeight. 
end; 


dxCI3  :■  dxmin: 
end: 

C  initialize  present 
fpb  :>  RefPosB: 

dyCI3  :•  dynin: 

foot  positions  ) 

<  initialize  H  matrix 
mith  HC1 3  do 

and  inverse 

matrix  INVH 

> 

begin  X  :»  1.0: 
eith  HC23  do 

Y  :■  o.o; 

z  :*  o.o: 

end: 

begin  X  !«  0.0; 
mith  MC33  do 

y  :«  i.o: 

z  :«  o.o: 

end: 

begin  X  S*  0.0; 
eith  HC63  do 

Y  :•  o.o: 

z  :•  i.o: 

end: 

begin  X  :■  10. 0: 
NatlnverseC  h,Invh  )J 

y  :»  lo.o 

:  Z  :•  NoainalHeight:  end: 

Filename  !>  'ETESPACE  *J 

r«i*t(  F. Filename, *.dat; 1*  )J  <  open  file  for  SyeSpace  > 
for  I  :*  1  to  *  do 

begin  <  read  in  vehicle  data  file  > 
for  J  :■  1  te  4  do 

readC  F.£ye5paceCI,J3  )> 
readlnC  F  )t 
endt 

CloseC  F  >5 


Filename  :■  'PLANES  ' • 

resetC  6, Filename, '.OAT: 1'  }:  (  open  data  file  for  coefficients  of  planes  > 

for  I  :•  1  to  MaxPolys  do  (  read  in  Planes  > 

begin 

PlanesCIl  :>  G»:  getC  G  )? 
end: 

CloseC  G  ): 


<  read  in  vehicle  data  file  > 

Filename  !*  'Vehicle  '; 

ReadObJectC  Filename ,NumPolys, PointstPelygens, Vertices  ): 
for  I  :•  1  to  16  do  C  store  points  of  body  for  analysis) 

Body Cl 3  :>  PointsCI3; 

WorkVolumeC  Polygons. Vertices, Body  >: 

PilNua  :■  2:  NamFilC  PilNum  )!  (  Display  file  number  for  vehicle  ) 

VehicleC  PPE.PPE.f alse , false, Contact ,H, dx.dy, Ref PosS.PPB. 

E ye Sp ace, NumPolys, Body (Points, Poly gens, Planes, Vert ices) i 

<  initialize  desired  foot  positions  and  estimating  points  > 
for  I  :•  1  to  6  do 

begin 
J  :• 


PP0ECI3  :•  PointsC19*J3; 

•nd  ; 

ppse  :»  ppoe; 

•nd;  (  ASVINIT  > 


procedure  HaltC  v a r  NVELX* 

NVELV. 

nrvelz  :  p#al; 

*ir  BodyTransRata • 

9odyRot at aRat a  :  Vector; 
var  PR8  :  Array6Vector 

>: 


p 

ft 


»ar  I  :  integer; 

begin  <  Halt  > 

<  initialize  velocity  vectors  ) 

NVELX  St  0.0;  NVELV  St  0.0;  NRVELZ 
•ith  BodyTransRata  do 
begin 

x  :*  o.o:  r  :»  o.o:  z  s»  o.o: 

end: 

•  ith  BodyRot aterate  do 
begin 

x  :■  o.o;  v  :*  o.o;  z  ;»  o.o; 

end; 


for  I  S*  1  to  4  do 
■ith  P RBCX3  do 
begin 
X  ;■  0.0; 
end; 


o.o; 


v  :»  o.o:  z  :»  o.o: 


■riteln: 

•ritalnC  CHR<27),'*6 - ASV8®  ST0PPE0 - ') S 

end;  (  Halt  > 


procedure  InitializoC  var  H  :  Matrix; 

var  dx • 


Oya 

RPhase, 

LPhase  :  Array®; 
var  RefPosS* 

PPOE* 

PPB  ;  Array 6Vec tor ; 
var  PootLifta  Phase* 

Period* 

Beta  s  real! 
var  Contact  t  Tactile; 
var  EstiaatePlag  :  boolean: 
var  LiftoffPlsg* 

PlacePlag  t  Arr ayABoolean: 
var  EyaSpace  l  Matrix®; 
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var  NuePolys  :  integer; 

var  Body  i  Array 52V»ctor ; 

war  Points  t  ArrayNaxPtsVector; 

var  Polygons  :  ArrayMaxPolysPolygont 

var  Pianos  !  ArrayMaxPolysOnePlanet 

var  Vorticos  :  ArrayMaxVtcesXnteger 


LogStatos  *  (  Liftoff*  TransforPoroard*  Placaaant  ); 


Poot 

TPhase*  TRTiao 


:  intogort 
:  real! 


LogStato 


:  LogStatos: 
:  factor i 


begin  (  Initialize  procedure  body  > 
eritelnt 

eritelnC  CHRC27),  *«6  INITIALIZING  ...•)» 

Phase  :■  0*0;  <  initialize  kineaatic  cycle  Phase  > 

Period  :*  1000. OS 

TRTiao  :»  (  1.0  -  Beta  )  *  absC  Period  ): 

(  coapute  desired  initial  foot  heights  ) 
for  Poot  :•  1  to  i  do 
begin 

(  initialize  legphase  and  desired  foot  positlene  > 
LPheseCFootl  !■  Phese  -  RPhaseCPootS  a  i.o; 
LPhaseCPootl  :«  LPhaseCPootl  -  truncC  LPhaseCPootl  >* 

if  LPhaseC  Poet  1  <  Beta 


begin  (  support  phase  1 

Contact  :«  Contact  a  C  Poet  31  t  Initialize  contact  state  > 

PPBC  Peot  3.Z  la  -NoainelHeight : 

<  turn  off  all  flags  of  transfer  leg  ) 

L if tof f PlagCPoet]  t«  falsel 
PlacePlagCPeetl  !■  falsel 
end 


else  begin  <  transfer  phase  > 

Centect  :«  Contact  -  C  Peat  3t 

TPhase  la  t  LPhaseC  Peet  3  -  Seta  >  /  <  1.0  -  Beta  >; 

if  TPhase  <  LiftPhese  (  specify  leg  state  > 
then  LegStete  t>  Liftoff 
else  if  TPhase  >  PlscePhase 

then  LegState  :■  Placeaent 
else  LegState  la  TransferPon 


cese  LegState  ef 

Liftoff*  begin 

PPOCCPeetl.Z  «•  PootLift! 

Liftoff PlagCPootl  Ja.truel 
PlecePlagCPoot]  *e  false* 

PPBCPeet3.Z  la-NeeinalHeight  a  PootLift 


*v 


■I 


I 

/r. 


end: 


*  TPhase  /  LiftPhase: 


TnnsfirFsrfird:  begin 

Liftoff FI agCFootl  .'*  false; 
PlaeaPlagCFootl  :»  falsa; 

FPBCFoo t3.Z  :*  -NoainalHeight  ♦  FootLlft; 
and: 

Flacaaants  bagin 

FPOECFootO. z  :«  o.o; 

Lif tOf f FlagCFootl  :«  falsat 
FlacaFlagCFoet3  *•  trua; 

FPSCFootO.Z  ! *  -NoainalHaight  ♦  FootLift 
a(  1.0-TPhase  )/<1.0-PlacePI 

and: 

and;  (  easa  and  ) 
and; 


>): 


and; 


VehicleC  FPE*FPE* false* falsa 'Contact ,H,da»dy «Ref PosB*FPE, 

Eye  Space* NgaPolys* Body (Points 'Polygons* PI  anas 'Vertices  ) : 

(  initialize  terrain  estiaation  flag  > 

EstiaataFlag  !»  true; 

ar iteln; 

aritelnC  CHRC27).  *»6  INITIALIZATION  COMPLETE'): 
aritaln; 

and:  C  Initialize  > 


1 


1 


GHAITS 


HPGRAP.MAC 
NHA-JOON  LEE 
CALL  STATEMENT! 


.LIST  TTN 
GTS  »  167760 
GTB  ■  167762 


10-27-82 

t  CALL  INITZ 

CALL  NAMPILCPILNUM) 
CALL  STPNAM 
CALL  STOVER(FILNUM) 
CALL  NOVETOCtX.IO 
CALL  DRANTOXIX.IY) 

{GRAPHICS  TRANSLATOR  STATUS 
{GRAPHICS  TRANSLATOR  BUFPER 


HACRO  TO  OUTPUT  OATA  TO  THE  HP  I/Q  DEVICE 


.MACRO 

OUTHP  T , 7GUAIT 

TSTB 

SAGTS 

BPL 

GUAXT 

3IC 

>0  02 ,3  ACTS 

MOV 

T.3AGTB 

BIS 

*002 ,3>GTS 

.ENOM 

(CHECK  STATUS  OP  HP 

{IP  BIT  7  IS  It  HP  IS  READY 

{CLEAR  BIT  1  COEVCMD) 

{HOVE  DATA  TO  OUTPUT  SUPPER 
{SET  BIT  1  POR  OATA  AVAILABLE 


{TO  INITIALIZE  HP  GRAPHICS  TRANSLATOR 

INXTZl S  MOV  AX  NX  T  *  R1 

HOV  AB..R2 

LOOP:  OUTHP  (Rl)t 

SOS  RZtLOOP 

RTS  PC 

INIT:  • UORO  1460 00 • 146000 t 101000 • 1 02000 

.MORO  100000 t 112000, 110000,115000 
{ 

{TO  NAME  OATA  PILE 


INIT: 


nahpil:: 


TST  CR5>* 
MOV  3(R5)«,R1 
*00  >107000,21 

OUTHP  Rl 
RTS  PC 

; 

1T0  STOP  NAMING  PILE 
1 


STpnam:: 


{PILE  NUMBER 
{NAME  PILE 


OUTHP  >112000  {STOP  NAMING  PILE 

RTS  PC 
{ 

{TO  MOVE  POINTER  TO  THE  BEGINNING  OP  PILE 


(GET  PILE  NUMBER 
1PIN0  PILE 

{OUTPUT  'PINO  PILE  COMMAND ‘ 


STOVER::  TST  <R5)> 

MOV  3CRS)*,R1  {GET  PILE  NUMBER 

*00  >193000,21  {PINO  PILE 

RTS**  Pl  10UTPUT  'PINO  PILE  CO 

{ 

{TO  MOVE  THE  CURRENT  BEAM  POSITION  TO  A  POINT 
{TO  START  ORAMING. 

: 

NOVRTOU  TST  <RS>* 

OUTHP  >143000  {PEN  CONTROL 


QUTHP  *1*1003 
OUTMP  3CR53* 
OUTMP  3(R5>* 
RTS  PC 


:pen  up 

{GET  IX 
•  GET  IT 


:to  generate  a  visible  vector  prom  the  current 

(BEAM  POSITION  TO  A  POINT 


3RANTO:: 


OUTHP  *1*1001 
OUTHP  3CR5)* 
OUTHP  3CR5)* 


<R5>* 


iPEN  DOWN 

:get  ix 

•GET  IT 


TO  ERASE  ALL  DATA  OP  A  PILE 

ERASE  SHOULO  3E  POLLOMEO  BT  STOVER  TO  PUT  NEW  DATA 
ON  THE  ERASED  PILE 


erase::  tst  crsj* 

NOV  3CR5)*,R1 

AOO  *103000, R1 

OUTHP  R1 
RTS  PC 


{PILE  NUMBER 
’.ERASE  PILE 
:OUTPUT  *EP  COMMAND* 


PINO  ABSOLUTE  LOCATION  IN  OISPLAT  BUFFER 


pimol:: 


TST  <R5>* 

NOV  3<RS)*,R1 

OUTHP  *132000 

OUTHP  R1 

RTS  PC 

: 

•OISPLAT  TEXT  OP  STRlNGl 
{CALLING — TEXTCVAR  IX, IT 
: 

TST  CR5)* 

OUTHP  *143000 

OUTHP  *141000 

OUTHP  OCRS)* 

OUTHP  3CR5>* 

OUTHP  *141001 

MOV  a<R3)*,Rl 

*09  *145000, R1 

OUTMP  R1 

OUTHP  *140000 

MOV  aCR5)4,R4 

MOV  (RS3+.R3 

LOOPl:  MOVB  (R3)+,R2 

1IC  *1TT600,R2 

OUTHP  R2 

SOS  R4, LOOPl 

OUTHP  *146000 

RTS  PC 

< 

{OISPLAT  TEXT  OP  STRING2 
{CALLING— TEXTCVAR  IX,  IT 
1 

TEXT2:*  TST  CBS)* 

OUTHP  *143000 

OUTHP  *141000 


TEXTl: :  TST 

OUTHP 


{GET  LOCATION 
(OUTPUT  'PL  COMMANO* 
{OUTPUT  LOCATION 


,size,nchar:  integer:  var  tx:  stringi  j 


{USING  PEN  CONTROL 

{PEN  UP 

{IX 

{IT 

{PEN  DOWN 
{SIZE 

{CHAR  SIZE  ANO  ROTATION 
{TO  HP 

{COMMANO  FOR  'TEXT' 

SNCHAR 

{ADDRESS  OP  FIRST  ELEMENT  OP  TEXT 
{GET  A  CHAR 
{CLEAR  PARITT  SIT 
{TO  HP 

:sno  op  text 


,sizs,nchar:  integer;  var  tx:  stringz  > 


{USING  PEN  CONTROL 
{PEN  UP 


L00P2: 


OUTMP 

«(«>♦ 

SIX 

OUTMP 

IX  V 

OUTMP 

•141001 

(PEN  OOUN 

mov 

3<RS>«,R1 

tSIZ! 

*00 

•145000. ai 

(CHAR  SIZE  ANO  ROTATION 

OUTMP 

R1 

JT3  HP 

OUTMP 

•140000 

JCONNANO  POR  ‘TEXT* 

NOV 

a<RS)4.R4 

tNCHAR 

MOV 

<RS)*.R3 

•AOORESS  OP  PIRST  ELEMENT  OP  TEXT 

“OVS 

(R3>4.R2 

SGET  A  CHAR 

SIC 

•177600, R2 

(CLEAR  PARITY  SIT 

OUTMP 

R2 

•  TO  HP 

SOS 

R4.L00P2 

OUTMP 

•146000 

(END  OP  TEXT 

STS 

PC 

.ENO 
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{eeeee**eeee*e*eeeee*eeeeeeaeeeeeeeeaeee*eeeeeee*e*e*e as****** a*********) 

<  File:  TER0IS.P4S  > 

(  This  fils  is  for  perspectiwe  display  of  tho  prlsaatic  terrain.  > 

t  > 

<**********************•**•***** ** ****** ************** ****** ****** ******> 

Prograe  TEROIS: 

tlnclude  Global: 

{include  Mainwar; 

{include  OISP; 

{include  Joystick: 
tincludo  LibrJ 

const  OneOegree  *  017453:  <  1  dograo  in  rad  > 

Tuidth  *  60. 0:  <  T-oidth  of  torrain  in  foot  > 

Spaco  *  1.0:  <  distance  batooon  too  linos  in  torrain  display  > 

Cntrlnt  •  VoctorC  20.0.30.0,0.0  ); 

ft  ■  200.0:  <  distance  batooon  EP  and  Cantor  of  interest  > 

var  FilNuo,  It  J  :  integer; 

Pilonaao  :  String; 

EyoPt  :  Vector; 

Jscale*  0T,  Last*  Present* 
tangle*  Tangle*  HR 

:  real: 

Velocity  :  array3; 

p  :  test: 

G  :  file  of  OnePlane: 

procedure  Initz:  NonPascal; 

procedure  NaaFilC  war  FilNuo  :  integer  >:  NonPascal: 
procedure  StpNaa:  NonPascal; 

procedure  StowerC  war  PilNua  :  integer  >;  NonPascal: 
procedure  PowetoC  war  XX*IT  i  integer  );  Non°ascal; 
procedure  OraetoC  war  IX*IT  :  Integer  );  NonPascal: 

procedure  ReadTorraln; 

<  global  aodified  J  NuaPolys*  Points  > 

war  X*  J* 

VI*  V2 .  V3 •  V4,  NuaPts* 

:  Integer; 

Pilenaoe 

:  string; 

P.  6 

:  test: 

begin 

*ilenaae  :»  'Terrain 

resetC  P. Pilenaoe. '081 : C3S7, 1 3TERRAIN.04T'  >1  (  open  input  file  > 

Pilenaoe  :■  'tr4.dat  'I  C  output  fllenane  for  display  data  > 

reerite<  GtPilenaae* *.3»T'  );  (  create  outout  file  ) 

readlnC  P, NuaPts  )i  TapNuapts  :*  NuaPts; 

NuaPts  :•  2*NuaPts;  NuaPolys  :■  TapNuaPts  -  l; 

(  generate  a  nea  file  for  polygon  description  of  terrain  > 
eritelnC  G*NuaPts:3*'  **NuaPolys:3>; 

for  I 1*1  to  TapNuaPts  do  (  description  of  points  > 

aith  PolntsCXS  do 


I 

) 

« 


n 


L 


s 

Ll 

$ 

;V 

# 


V  *1 

"ol 

'$j 

>5 


begin 

readlnC  F.J.X.2  >:  r  :•  0.0! 

eritelnC  G,j!3,'  *,X!8!3.*  *,r:8:3,*  M:i:3); 
•  nd; 


for  I!»l  to  TepNuePts  do  (  description  of  points  > 

•ith  Point*CI3  do 
begin 

J  !«  I  ♦  TopNuoPts!  T  :»  Yuidthi 

aritelnt  (•J!3t*  '»X:8:3.*  *,V:S:3»*  *.Z!9:3)! 

end! 


J  *•  *!  <  nuober  of  edges  > 

for  X!»l  to  NuoPolys  do  <  vertex  descriptions  > 

begin 

VI  !■  I!  V2  !■  XeTepNuePtS!  V3  !«  V2*l;  V*  tm  I ♦ 1 ! 
eritelnC  6.JI3.'  *,Vi:3,*  *.V2!3.*  *.V3:3.'  *,V*:3  )! 
end! 

Cloeet  P  )i  <  close  terrain  inforoation  file  > 

Closet  5)5  <  close  terrain  data  file  > 

<  ReadTerrain  > 


orocedure  GetPlanesi 

t  global  referenced:  NuaPolyst  Polygons*  Points*  Vertices 
(  aodified!  Planes  ) 

war  Al.  31.  Cl.  N 

:  real: 

Jl.  J2,  J3«  t 

:  integer! 

Rtl.  Pt2 .UnitNoreal 
:  vector! 


begin 


for  i:»l  to  NuoPolys  do 
begin 

eith  PolygonsCX3  do 
begin 

Jl  :>  VerticesC  Start*l  3! 

J2  :•  VerticesC  Starts  3! 

J3  i«  VerticesC  StartePolyVtces  3! 

end! 

eith  PlanesCX3  do  <  plane  coefficients  > 

begin 

VectSubt  PointsC J33»PointsC Jl 3*Ptl  )t 
VectSubt  PointsCJ23.PointsCJ13.Pt2  >! 
CrossProdt  Ptl.Pt^, UnitNoreal  >! 

Cist  :«-0otProdC  UnitNoreal .PointsCJl 3  )! 

t  boundaries  of  a  plane  > 

Rein  la  PointsCJ13«X!  Xaax  :»  PointsC J3 3. X ! 
if  X  ■  NuoPolys  then  Xeax  :■  1000*0! 

end! 

end!  <  for  loop  > 

<  GetPlanes  > 


orocedure  Spacingt  Ntl  Matrix*  3i 


& 


<  global  rof oroncto:  NuaPolys*  Points*  Ysidth*  Spaco  > 

wap  TaoPt*  NesPt 

s  Voctop; 

XO*  10*  HO*  Slops*  Coax* 
xo is •  Zdis 

:  pool: 

I*  J.  IX.  IT 

:  intogop; 

Plagl*  Flag! 

:  booloant 

bsgin 

top  l:»l  to  NuaPolys  do 
bsgin 

XO  :«  PointsCI*13.x  -  PointsCI3.x; 

10  :•  PointsCI*13.Z  -  PointsCIl.Z; 
ho  :«  sqptc  xooxo  ♦  zo*io  ): 
if  XO  <>  0.0 
thsn  bsgin 

Sloao  t»  Z0/X0S  CotX  S»  XO/HDi 

Xdis  t*  CosX*Spaes.  Zdis  :•  Slopssxdis: 

and 

sis*  bsgin 

Xdis  :«  o.o; 

if  10  >  0.0  thsn  Zdis  i*  Spaea 
slss  Zdis  :*-Spac*J 

sndj 

sith  PointsCIO  do 
bsgin 

TapPt.X  ;•  XsXdisI  TapPt.Y  S»  0.0;  TapPt.Z  *»  Z+Zdis; 
sndt 

rspsat 

Voct*Tp*nsfopaC  Ht»T*pPt,N**Pt  >S 
HaksOlsplayablsC  MsaPt  >; 
with  NsaPt  do 
bsgin 

IX  :«  tPune(X);  IV  ?■  tPuncCY):  MovotoCIX.IY); 
and. 

TapPt.Y  !■  Ysidth; 

V*et*TpansfoPa<  Nt«T*pPt*N*oPt  )i 
MaksOisplay abls(  NsaPt  )J 
sith  NsaPt  do 
bsgin 

IX  :*  trunc(X);  IV  S«  truncCY).  OraatoCIX.IY): 
and; 

sith  TspPt  do 
bogin 

x  :>  xsxdis;  v  :»  o.o;  z  :■  z*Zdis; 

and; 

Plagl  :■  falsa;  PlagZ  :*  false; 
if  Z0  >  0.0 

thsn  bsgin 

if  TapPt.Z  >  PointsCX+13.1 
thsn  Plagl  ;■  tpus 
slss  Plagl  ;•  falsa; 
and 


also  begin 

if  CTapPt.Z  <  PointiCI*i3.Z) 
or  CTapPt.X  >  PointaCI*13.X> 
then  FXagZ  S»  true 
•l*o  PlagZ  :*  false: 

and 

until  Plagl  or  PlagZi 

and :  <  for  loop  > 

C  dr ■■  base  of  terrain  > 

Vect4TransforeC  *t ,PointsC13, NaaPt  )t 
MakeOisplayableC  NaaPt  )i 
aith  NaaPt  do 
bagin 

IX  :■  truncCX) ;  XT  :■  trunc(Y):  NovetoCIX.IY): 
and: 

aith  TapPt  do 
bagin 

x  :»  o.o:  y  :■  o.o:  z  :»  -10.0: 

and: 

Vaet4Tranifora<  *t,TapPt»NeaPt  )! 

NakeOisplayableC  NaaPt  )■ 
aith  NaaPt  do 
bagin 

IX  :■  truncCX);  IY  :*  truncCY):  OraatoCIX* IY ) J 
and: 

aith  TapPt  do  X  :■  PointsCNuaPolys*13.X; 
Veet4TransforaC  Nt , TapPt. NaaPt  ): 

NakeOisplayableC  NaaPt  ): 
aith  NaaPt  do 
bagin 

IX  :•  trunc(X):  IY  :•  truncCY):  OraatoCIX, IY): 
and: 

Vect4TransforaC  Mt.PointsCNunPolys*13,NeoPt  ): 
NakeOisplayableC  NaaPt  ): 
aith  NaaPt  do 
bagin 

IX  :•  truncCX) ;  IY  :•  trunc(Y):  OraatoClX • IY) : 
and: 

for  I  :■  1  to  Z  do 
bagin 

aith  TapPt  do 
oagin 

x  :•  o.o:  y  :«  o.o:  z  :•  -io.o; 

if  X  »  2  than  X  :»  PointsCNuaPolysall.x: 
and: 

Vaet4Tranafora<  Ht, TapPt .NaaPt  )l 
NakeOitplayableC  NaaPt  X 
aith  NaaPt  do 
bagin 

XI  :a  truncCX) :  XT  :•  truncCY):  NovatoCIX(XY): 
and: 

aith  TapPt  do  Y  :•  Yaidth: 

Voct4TrsnoforaC  Nt.TapPt, NaaPt  X 
"akeOisplayableC  NaaPt  X 
aith  NaaPt  do 
bagin 

IX  !•  truncCX):  XT  :■  truncCY):  9raatoCXX,XY>: 


aith  TmpPt  do 
begin 

2  !*  o.o:  if  I  ■  2  th#n  2  !*  Point sCNuaPalys+1 3.Z • 
end; 

VectATransfora!  *<t.TnpPt, NaaPt  >i 
NakeOlsplayable!  NaaPt  )! 
aith  NaaPt  do 
begin 

IX  !»  trunc(X)  :  IT  S *  tPuncC Y) 5  Or sato! IX , IY > : 
and; 

and:  <  for  is«  I  to  2  do  > 

*nd>  {  Spacing  > 

bagin  (  TEROIS  > 

Initsl  PilNua  :>  i;  NaaFiUFilNua) ; 

ReadTerrain; {  raad  in  tarrain  Information  and  make  tarrain  data  fila  > 
Filename  :*  'tr4.dat  *; 

ReadObject!  Filanaaa  )i  (  raad  in  tarrain  data  fila  > 

GetPlanes: 

<  gat  ayaspaca  by  using  joystick  > 

Xangla  !«  -90.0;  Yangla  :■  10. 0:  !  initialize  ays  point  > 

Jscale  !«  S.O:  Prasant  i«  Tima; 

rapaat 

Joystick!  Velocity •  Jscale  1: 

Last:*  Prasant; 

Prasant  S»  Tiael 

OT  ;«  3600.0  a  !  Prasant  -  Last  >; 

Xangla  S«  Xangla  a  0T  a  VelocityCOi: 

Yangla  :■  Yangla  a  OT  *  ValocityCli: 
aith  EyaPt  do 
bagin 

HR  ;■  R  *  Cos!  OnaOagraa  *  Yangla  ); 

X  !*  HR  a  Cos!  OnaOagraa  a  xangla  >  -  Cntrlnt.x: 

V  :■  HR  a  Sin!  OnaOagraa  a  Xangla  >  -  Cntrlnt.v: 

l  :*  R  a  Sin!  OnaOagraa  a  Yangla  >  -  Cntrlnt.z: 

and; 

GetEyeSpaca!  EyePt .Cntrlnt  )i 
Stavar!  FilNua  )■ 

Display  Object!  EyaSpaca  .-false  ):  !  display  tarrain  ) 

Spacing!  EyaSpaca  );  (  display  spacings  > 

StpNaa; 

until  !  ValocityC23  <  -2.5  ): 

Fila name  !«  'EYE SPACE  *: 
raarita!  F.Fllanaa*. *.0AT; 1*  ); 
far  I  :■  1  to  4  do 
bagin 

for  J  :«  1  to  4  do 

arita!  F.EyaSpacaCI, J3  >; 
aritalnC  F  >: 
and! 

Close!  P  >; 

File name  :«  'PLANES  'I  <  PLANES. CAT:i  > 

raarita!  6f Filename* '.DAT; 1*  )t  !  create  file  for  coaf f iclants) 

for  I  )•  1  to  NaxPolys  do  !  of  pianos  of  'terrain  > 

begin 

G*  ;•  PlanasCI3:  put!  S  >; 
and: 

Close!  S  ): 

and. 


c  sysspace.oat;i  > 

!  craata  fila  for  EyaSpaca  > 
!  store  data  in  fila  > 


<  TEROrS  > 


(******************************************«*•*****«** ********** ******««) 

<  Fli*!  TERPRF.PAS  > 

(  This  fill  is  for  torrain  profila  gtnirition.  > 

<  > 

(*****«*»«** ****** **************** **************************** *»**«***«*> 

orogra*  TERPRF; 

^include  Global; 
tinclude  PainvarS 
tine lude  OISP; 
tincluda  Joystick; 
tineluda  Libr; 


tyo# 


var 


Stringl  *  oackad  arrayC1..113  of  char; 
String*  •  packed  arrayCl*.23  of  char; 


IX.  IY 


:  integer; 


orocadura  InittS  NonPascal; 
procedure  NaeFil!  ear  FilNu* 
procedure  StpNa*;  NonPascal; 
procedure  Stover!  «ar  Pilfcua 
procedure  Noveto!  war  IX, IY 
procedure  Oraoto!  ver  IX, IY 
procedure  Erase!  var  FilNu* 
procedure  FindL!  war  Locate 


integer  >;  NonPascal; 


:  integer  );  NonPascal; 
integer  )i  NonPascal; 
integer  )t  NonPascal: 
integer  >;  NonPascal: 
integer  >:  NonPascal: 
procedure  Teatl<  ear  IX, lY,Char$ise,Nchar:  integer;  var  TX:  Stringl  ); 
NonPascal: 

procedure  Text*!  var  IX, IY ,CharSize,Nchar:  integer:  var  Tx:  String*  ); 
NonPascal: 


procedure  Cursor!  var  IX, IV:  integer  >: 
!  dr a,  arros  cursor  on  screen  > 


const  L  *  205 

var  KX,KY,PileNu«  :  integers. 


begin 


end; 


FileNus  :>  *: 

xx  :«  tx  -  l: 

Oraoto!  IX, XY 

xx  :>  ix  ♦  l; 
StpNa*: 

!  Cursor  » 


NaaFil!  FileNue  ): 

XV  1»  IY  -  L5  Hoveto! 

>; 

Oraoto!  XX, XY  >; 


XX, KY 


); 


procedure  NoveCursor!  var  IX,IY  :  integer  >: 
!  Move  cursor  on  screen  > 


var  Present,  Last,  Scale,  3T 
FileNuo  :  integer; 
Velocity  :  arrays; 

begin 

■ileNu*  :■  4; 

Scale  :«  *90.0; 

Present  !■  Tlae; 


real; 


M 

y 

1**; 


!  scale  for  joystick  ) 


>s»v» 


repeat  C  nova  cursor  aithout  draeing  vactors  > 

Joystick!  Valocity .Scala  ); 

Last  :«  Prasant; 

Present  :»  Tlaai 

QT  :*  3600.0  *  C  Prasant  -  Last  ){ 

IX  :«  trunc!  0T*VeiocityC03  >  ♦  IX: 

IT  :>  truncC  0T*VelocityCl 3  )  ♦  IT! 

Stover!  FilaNua  )! 

Cursor!  IX, IT  ) 

until  (  VelocityC23  <  -100.0  ):  (  until  z  axis  is  tsistad  clockaise  ) 
ahila  ValocityC23  <  -70.0  do  <  aait  until  joystick  returns  > 
Joystick!  Valocity, Seals  );<  to  natural  position  > 

!  koveCursor  > 


orocedure  Oraakenu; 


begin 


FilaNua,  IX,  IT, 

CS,  NC  :  integer: 

TX  :  Stringi: 

FilaNua  :«  i:  NaaFil!  FilaNua  ): 

ix  :«  o:  it  :■  soo: 

kovato!  IX, IT  ): 

IT  :a  1000:  Oraato!  IX, IT  ): 

IX  :»  400:  Oraato!  IX, IT  ): 

IT  :«  800:  Oraato!  IX, IT  ): 

IX  : ■  o:  Oraato!  IX, IT  ): 

ix  : ■  200:  IT  :•  1000;  kovato!  ix.it  ): 

IT  :•  300:  Oraato!  IX, IT  ): 

NC  :«  11:  CS  :■  o:  !  sat  character  size  > 

IX  :•  34:  IT  :«  no:  rx  :«  *  oraa  ': 

Taxtl!  IX, IT.CS, NC.rx  )! 

ix  :«  341  IT  :*  360:  TX  :■  *  Terrain  *: 

Taxtl!  IX,IT,CS,NC,TX  )l 
ix  :>  234:  it  :«  oio:  rx  :»  'Perspective': 
Taxtl!  IX, IT.CS, NC,TX  >: 

IX  :■  234:  IT  :•  860:  Tx  :•  *  visa 
Taxtl!  IX.IT.CS, NC , T X  ); 

StpNaa: 

!  Oraakanu  > 


procedure  Oraaxz; 

!  d ram  X  and  2  axis  to  draa  cross  section  of  terrain  > 

const  Xain  ■  30:  Xaax  *  300: 

Tain  ■  330:  Taax  ■  600: 

var  I,  IX,  IT,  FilaNua, 

J,  JX,  JT,  CS,  NC 
:  integer: 

TX  :  String2: 


begin 


FilaNua  :•  2:  NaaFil!  FilaNua  ): 

NC  !•  2t  !  nuabar  of  character  > 

CS  :■  II  !  character  size  > 

for  I-  :■  1  to  2  do  !  draa  XZ  axis  > 

begin 

IX  :•  Xain:  IT  :■  Tain:  kovato!  IX, IT  ): 


If  I  *  1  then  begin 


ix  :» 

end 

Xnax: 

jx  :■ 

ix,7: 

JY 

:«  iy-12: 

tx  :«*x 

else  begin 

IY  !» 

Yarns: 

jx  :> 

ix-8: 

JY 

:■  IY*7; 

tx  :«*i 

and: 

OraetoC  IX, If  ){ 

Text2C  JX, jy.CS, NC.TX  )J 
and! 

cs  :«  o; 

JX  :*  Xain :  jy  :«  fmin-25;  tx  :»*o  ':  Text2C  JX, JY.CS, NC.TX  ): 
for  I  :»  1  to  9  do  (  draa  scale  every  10  feet  on  X  axis  > 

begin  (  put  the  numbers  on  them  > 

IX  !»  Xmin  ♦  I«100:  IY  :•  Ymin  ♦  5:  NovetoC  IX, IY  ): 

IY  :«  Ymin  -  5;  OraetoC  IX, IY  ): 

jx  :■  ix  -  10;  jy  :*  iy  -  20: 

case  I  of 

i:  Txie'io*;  2:  rx:«*20':  3:  tx:»*30*:  4:  tx:**40*: 

s:  Tx:»'50':  6:  tx:**60':  7:  tx:«*70*;  8:  Txt»*eo': 

end:  <  case  end  > 

TextZC  JX, JY.CS, NC.TX  )i 
if  I  «  8  then  begin 

JX  JX  ♦  35:  TXSe'ft':  Text2<  JX.JY.CS, NC.TX  >: 
end: 

end; 

for  I  :*  1  to  4  do  C  draa  scale  every  5  feet  on  2  axis  > 

begin  C  and  put  numbers  on  then  > 

IV  :«  Yrnin  ♦  1*50;  IX  :>  Xmin  ♦  5  :  NovetoC  IX, IV  >; 

IX  :•  Xmin  -  s:  OraetoC  IX, IY  >; 

jx  :■  ix  -  30;  jy  : ■  iv  -  6: 
case  I  of 

u  tx:»*  5*:  2:  tx:«'io*:  3:  Tx:»*i5*;  4:  tx:«*20*: 
end: 

Te*t2C  JX.JY.CS, NC.TX  >; 
if  I  «  4  then  begin 

JY  :■  JY»25:  Tx:»'ft»;  Toxt2C  JX.JY.CS. NC.TX  >: 
end; 

for  J  Je  i  to  32  do 
begin 

IX  :*  ix  ♦  20:  NovetoC  ix.iv  >J 
IX  :e  IX  ♦  5;  OraetoC  IX, IY  >; 
end! 

end! 

StoN am: 

end!  -  C  OramXZ  > 


procedure  DramTerrain: 

<  draa  lines  for  terrain  interactively  using  Joystick  > 
C  and  generate  an  output  file  far  terrain  information  > 

label  101 

const  Scale  ►  0*11  Xmin  ■  SO:  Ymin  «  350; 
tyoe 


var 


String  •  pecked  arrayC1..73  of  char: 

IX,  IY,  I,  J,  PUeNum,  L  I  integer; 
Pilenaaa  l  string: 


TapPts  :  arrayC1..303  of  Vector; 


ci laNua  :«  3:  NaaFil!  FilaNua  )! 

L  :■  500:  !  initializa  arita  pointar  for  torrain  display 

ix  :«  xain;  IT  :«  rain;  I  :•  1: 
aith  TspPtsCIl  do 
bagin 

x  o.o:  z  ;•  o.o; 

and; 

FindLCLJ;  Soveto!  IX*IT  )S  L  :■  L  ♦  IS 

'lo vaCursor (  IX.IY  )i 
ahila  !IX>*00>  or  IIYOOO)  do 
begin 

i  :■  i  *  1: 
aith  TepPtsCIl  do 
bagin 

x  :•  Seala  a  C  IX  -  Xain  >; 

Z  :•  Seala  *  {  IT  -  Tain  >; 
and; 

FindLIL):  Oraato!  IX.IY  );  L  :«  L  ♦  15 

PovaCursor!  IX.IY  ); 

and; 

StpNaa: 

if  IX  <  ZOO 

than  bagin 
Initz; 

OraaHanu;  Cursor!  IX. IT  >;0raaXZ:  t  arasa  pravio 

goto  10:  (  draa  again  > 

and; 

■ilenaee  ;■  'terrain': 

rear itet  F.Filanaaa. '.0*T'>:  C  eraata  output  fila  > 

eriteln!  P,IS3>:  <  arita  nuabsr  of  points  >• 

for  J  :■  l  to  I  do 

aith  T»pPtsCJ3  do 

eriteln!  F.JS3.'  ',XJ8:3,'  '.ZS8:3  >:  C  arita  x  and  Z 
close!  P  >; 

C  OraaTerrain  > 


<  TP  PUP  body  > 

Initz:  OraaManu: 
ix  :■  soo;  it  :■  soo; 

Cursor!  IX. IT  >; 

ahila  !  IY<800  }  or  !  IX>200  )  do 
MoveCursor!  IX.IY  ); 

OraaXZ; 

OraaTarrain: 


ta  > 


valuas  > 


(*«****************•***************************************«************: 

C  File:  OISP.P*S 

(  This  fils  is  for  graphics  routines 

i  1 

<♦**********************************************************************! 

procedure  ReedObJectC  var  Filename:  String  )S 

war  It  J  •  NuaVtces*  NuaPts  :  integer; 

f  :  text; 

begin 

reset(Pi  Pilenaaet  *.C*T*>S  (  open  input  file  > 

readln(F»NuaPtst  NuaPolys);  f  read  in  points  > 
for  I  :>  1  to  NuaPts  do 

eith  Pointed!  do 

readlnCP.  J,  X,  r,  n; 

MuaVtces  ;»  0;  (  initialize  size  of  ver tax  array  > 

for  I  :•  1  to  NuaPolys  do  f  read  in  polygon  descriptions  > 

eith  PolygonsCIl  do 
begin 

start  :■  NuaVtcesi  <  start  point  in  vertex  array  > 

readCPt  PolyVtces  >:  (  nueber  of  vertices  ) 

for  J  :>  1  to  PolyVtees  do  (  read  vertex  pointers  > 

readCPt  Vertices!  NuaVtces  ♦  J  3); 
readln(F);  (  go  to  next  line  of  input  > 

NuaVtces  :•  NuaVtces  ♦  PolyVtces; 
end; 

CloseCP) ;  <  close  the  file  > 

end;  <  ReadObject  > 


procedure  GetEyeSpaceC  EyePt*  Cntrlnt:  Vector  >; 

var  Ntx  :  Natrix*; 

C1.C2  :  Vector; 

Hypotenuse*Cos** Sin*  :  real; 

begin 

eith  EyePt  do 

Translate  -X Y,-Z , EyeSpece  >: 

VectSTrensforaC  EyeSpsce*CntrInt »C 1  >;  C  translate  center  of  interest  > 

eith  Cl  do  Hypotenuse  :•  sort!  xsx  ♦  T*T  }; 
if  Hypotenuse  <>  0*0  than 
begin 

Cos*  :•  Cl.T  /  Hypotenuses  Sin*  :»  Cl.x  /  Hypotenuse; 

RotateNatC  *Z'. Cos*. Sin*, Ntx  >;  <  rotate  about  z  axis  > 

Hat4.Hult(  Ntx .EyeSpace* Eye Space  >; 

endS 

VecttTransforaC  EyeSpace, CntrXnt,C2  >S  C  rotate  etr.  of  interest  » 

»ith  C2  do  Hypotenuse  S»  sqrtC  t*T. ♦  Z*Z  >S 
if  Hypotenuse  <>  0.0  then 
begin 

Cos*  !■  C2.T  /  Hypotenuse;  Sin*  !■  -C2.Z  /  Hypotenuse; 

RotateNatC  'x', Cos*. Sin*, Ntx  >;  C  rotate  about  x  axis  » 

NatONultC  Ntx .EyeSpace, Eye  Space  >; 

end; 

IdentC  Htx  )S 


(  switch  beteean  Y  and  Z  anas  > 
MtxC2,23  :«  9. OS  MtxC2«33  :»  -l.OS 
MtxC3,23  :«  1.0S  MtxC3,33  :«  0.0S 
Mat*Mult<  Htx,EyeSoace,EyeSpaca  )S 
•nds  (  GetEyeSpace  > 


proeadura  MakeOiaployablaC  war  Pt  :  Vactor  ); 

bajin  (  parapactiva  projaction  ) 

“t.x  :«  Seala  *  Pt.x  ♦  Middle. x: 

Pt.Y  :»  Seala  *  Pt.Y  ♦  Middle. Y; 
andS 


orocadura  OisplayOb JactC  war  Mt  :  Matrix*) ! 

war  TapPt  :  Vector; 

I • Jt X  a 

IX, iy  :  integer; 

begin 

for  I  :«  1  to  MuaPolya  do 

aith  PolygonaCIl  do 
begin 

Veet*Tranafora<  Mt,  PointaCVertieaeCStart*PolyVteee33«TapPt); 
MakeOiaplayableC  TapPt  >: 
aith  TapPt  do 
begin 

IX  :>  trunc(X) :  IT  :>  trune(V):  MowetoC IX , IY) ; 
and: 

for  J  ;■  1  to  PolyVteas  do 
begin 

Vect*Tranaf oral  Mt »Point«C VerticeaCStart*J33 , TapPt  >: 
MakeOiaplayableC  TapPt  )S 
aith  TapPt  do 
begin 

IX  S*  truncCX):  IY  ;»  trunc(V);  OraatoCIX.IT): 
and; 

and; 

end; 

and:  <  OisplayObJaet  > 


procedure  OiaplayPilaC  war  Mt  :  Matrix*): 

war  TapPt  :  Vector; 

I , J, K , 

IX, IY  :  integer: 

p  :  texts 

begin 

reeriteCP, 'ASVMP.dat *) ; 
for  I  :•  1  to  'luaPolya  do 

eith  PolygonaCIl  do 
begin 

VectOTranaforaC  Mt,  PointaCVerticeaCStart*PolyVtcea33, TapPt); 
MakeOiaplayableC  TapPt  >S 
aith  TapPt  do 

eritelnCP, '3 *, *  *,X,*  ',Y)S 
for  J  ;«  1  to  PolyVtcea  do 
begin 

Vect*Tranafora(  Mt,PointaCVertleeaCStart*J33,TapPt  >t 
MakeOiaplayableC  TapPt  )l 
eith  TapPt  do 

eritelnCP,'!*,*  '»*.'•  *.T>; 

and: 

end! 

cloaeC  P  )S 

and:  (  OiapleyPile  » 


